def main(): rospy.init_node('tf_test') rospy.loginfo("Node for testing actionlib server") #from_link = '/head_color_camera_l_link' #to_link = '/base_link' from_link = '/base_link' to_link = '/map' tfl = TransformListener() rospy.sleep(5) t = rospy.Time(0) mpose = PoseStamped() mpose.pose.position.x = 1 mpose.pose.position.y = 0 mpose.pose.position.z = 0 mpose.pose.orientation.x = 0 mpose.pose.orientation.y = 0 mpose.pose.orientation.z = 0 mpose.pose.orientation.w = 0 mpose.header.frame_id = from_link mpose.header.stamp = rospy.Time.now() rospy.sleep(5) mpose_transf = None rospy.loginfo('Waiting for transform for some time...') tfl.waitForTransform(to_link,from_link,t,rospy.Duration(5)) if tfl.canTransform(to_link,from_link,t): mpose_transf = tfl.transformPose(to_link,mpose) print mpose_transf else: rospy.logerr('Transformation is not possible!') sys.exit(0)
class TwistToPoseConverter(object): def __init__(self): self.ee_frame = rospy.get_param('~ee_frame', "/l_gripper_tool_frame") self.twist_sub = rospy.Subscriber('/twist_in', TwistStamped, self.twist_cb) self.pose_pub = rospy.Publisher('/pose_out', PoseStamped) self.tf_listener = TransformListener() def get_ee_pose(self, frame='/torso_lift_link', time=None): """Get current end effector pose from tf.""" try: now = rospy.Time.now() if time is None else time self.tf_listener.waitForTransform(frame, self.ee_frame, now, rospy.Duration(4.0)) pos, quat = self.tf_listener.lookupTransform(frame, self.ee_frame, now) except (LookupException, ConnectivityException, ExtrapolationException, Exception) as e: rospy.logwarn("[%s] TF Failure getting current end-effector pose:\r\n %s" %(rospy.get_name(), e)) return None, None return pos, quat def twist_cb(self, ts): """Get current end effector pose and augment with twist command.""" cur_pos, cur_quat = self.get_ee_pose(ts.header.frame_id) ps = PoseStamped() ps.header.frame_id = ts.header.frame_id ps.header.stamp = ts.header.stamp ps.pose.position.x = cur_pos[0] + ts.twist.linear.x ps.pose.position.y = cur_pos[1] + ts.twist.linear.y ps.pose.position.z = cur_pos[2] + ts.twist.linear.z twist_quat = trans.quaternion_from_euler(ts.twist.angular.x, ts.twist.angular.y, ts.twist.angular.z) final_quat = trans.quaternion_multiply(twist_quat, cur_quat) ps.pose.orientation = Quaternion(*final_quat) try: self.tf_listener.waitForTransform('/torso_lift_link', ps.header.frame_id, ps.header.stamp, rospy.Duration(3.0)) pose_out = self.tf_listener.transformPose('/torso_lift_link', ps) self.pose_pub.publish(pose_out) except (LookupException, ConnectivityException, ExtrapolationException, Exception) as e: rospy.logwarn("[%s] TF Failure transforming goal pose to torso_lift_link:\r\n %s" %(rospy.get_name(), e))
class calculate_goal_pose(smach.State): def __init__(self): smach.State.__init__(self, outcomes=['succeeded'], input_keys=['marker_pose', 'goal_pose'], output_keys=['goal_pose']) self.tf = TransformListener(True, rospy.Duration(5)) self.DISTANCE = 0.40 def execute(self, userdata): userdata.marker_pose.header.stamp = rospy.Time.now() pose = self.tf.transformPose('/base_link', userdata.marker_pose) p = [pose.pose.position.x, pose.pose.position.y, pose.pose.position.z] q = [pose.pose.orientation.x, pose.pose.orientation.y, pose.pose.orientation.z, pose.pose.orientation.w] rm = tfs.quaternion_matrix(q) # assemble a new coordinate frame that has z-axis aligned with # the vertical direction and x-axis facing the z-axis of the # original frame z = rm[:3, 2] z[2] = 0 axis_x = tfs.unit_vector(z) axis_z = tfs.unit_vector([0, 0, 1]) axis_y = np.cross(axis_x, axis_z) rm = np.vstack((axis_x, axis_y, axis_z)).transpose() # shift the pose along the x-axis p += np.dot(rm, [self.DISTANCE, 0, 0])[:3] userdata.goal_pose = pose userdata.goal_pose.pose.position.x = p[0] userdata.goal_pose.pose.position.y = p[1] userdata.goal_pose.pose.position.z = p[2] yaw = tfs.euler_from_matrix(rm)[2] q = tfs.quaternion_from_euler(0, 0, yaw - math.pi) userdata.goal_pose.pose.orientation.x = q[0] userdata.goal_pose.pose.orientation.y = q[1] userdata.goal_pose.pose.orientation.z = q[2] userdata.goal_pose.pose.orientation.w = q[3] return 'succeeded'
class HandHoldTeleop: def __init__(self): self.listener = TransformListener() self.rate = rospy.Rate(100) self.pub = rospy.Publisher('/cmd_vel', Twist) self.root = rospy.get_param('root_link','arm_link') self.tip = rospy.get_param('tip_link','gripper_link') self.x_home = rospy.get_param('x_home', 0.35) self.z_home = rospy.get_param('z_home', -0.2) self.x_rate = rospy.get_param('x_rate', 8.0) self.r_rate = rospy.get_param('r_rate', 5.0) rospy.loginfo("Started") while not rospy.is_shutdown(): gripper_pose = PoseStamped() gripper_pose.header.frame_id = self.tip try: pose = self.listener.transformPose(self.root, gripper_pose).pose if pose.position.z > self.z_home: msg = Twist() msg.linear.x = (pose.position.x - self.x_home) * self.x_rate msg.angular.z = pose.position.y * self.r_rate print msg.linear.x, msg.angular.z self.pub.publish(msg) else: self.pub.publish(Twist()) except: pass #rospy.logerr("Transform failed") self.rate.sleep(); self.pub.publish(Twist())
class ArmActions(): def __init__(self): rospy.init_node('left_arm_actions') self.tfl = TransformListener() self.aul = l_arm_utils.ArmUtils(self.tfl) #self.fth = ft_handler.FTHandler() rospy.loginfo("Waiting for l_utility_frame_service") try: rospy.wait_for_service('/l_utility_frame_update', 7.0) self.aul.update_frame = rospy.ServiceProxy('/l_utility_frame_update', FrameUpdate) rospy.loginfo("Found l_utility_frame_service") except: rospy.logwarn("Left Utility Frame Service Not available") rospy.loginfo('Waiting for Pixel_2_3d Service') try: rospy.wait_for_service('/pixel_2_3d', 7.0) self.p23d_proxy = rospy.ServiceProxy('/pixel_2_3d', Pixel23d, True) rospy.loginfo("Found pixel_2_3d service") except: rospy.logwarn("Pixel_2_3d Service Not available") #Low-level motion requests: pass directly to arm_utils rospy.Subscriber('wt_left_arm_pose_commands', Point, self.torso_frame_move) rospy.Subscriber('wt_left_arm_angle_commands', JointTrajectoryPoint, self.aul.send_traj_point) #More complex motion scripts, defined here using arm_util functions rospy.Subscriber('norm_approach_left', PoseStamped, self.norm_approach) rospy.Subscriber('wt_grasp_left_goal', PoseStamped, self.grasp) #rospy.Subscriber('wt_wipe_left_goals', PoseStamped, self.wipe) rospy.Subscriber('wt_wipe_left_goals', PoseStamped, self.force_wipe_agg) rospy.Subscriber('wt_swipe_left_goals', PoseStamped, self.swipe) rospy.Subscriber('wt_lin_move_left', Float32, self.hand_move) rospy.Subscriber('wt_adjust_elbow_left', Float32, self.aul.adjust_elbow) rospy.Subscriber('wt_surf_wipe_left_points', Point, self.prep_surf_wipe) rospy.Subscriber('wt_poke_left_point', PoseStamped, self.poke) rospy.Subscriber('wt_contact_approach_left', PoseStamped, self.approach_to_contact) self.wt_log_out = rospy.Publisher('wt_log_out', String) self.test_pose = rospy.Publisher('test_pose', PoseStamped, latch=True) self.say = rospy.Publisher('text_to_say', String) self.wipe_started = False self.surf_wipe_started = False self.wipe_ends = [PoseStamped(), PoseStamped()] #FORCE_TORQUE ADDITIONS rospy.Subscriber('netft_gravity_zeroing/wrench_zeroed', WrenchStamped, self.ft_preprocess) self.rezero_wrench = rospy.Publisher('netft_gravity_zeroing/rezero_wrench', Bool) #rospy.Subscriber('ft_data_pm_adjusted', WrenchStamped, self.ft_preprocess) rospy.Subscriber('wt_ft_goal', Float32, self.set_force_goal) self.wt_force_out = rospy.Publisher('wt_force_out', Float32) self.ft_rate_limit = rospy.Rate(30) self.ft = WrenchStamped() self.ft_mag = 0. self.ft_case = None self.force_goal_mean = 1.42 self.force_goal_std= 0.625 self.stop_maintain = False self.force_wipe_started = False self.force_wipe_start = PoseStamped() self.force_wipe_appr = PoseStamped() def set_force_goal(self, msg): self.force_goal_mean = msg.data def ft_preprocess(self, ft): self.ft = ft self.ft_mag = math.sqrt(ft.wrench.force.x**2 + ft.wrench.force.y**2 + ft.wrench.force.z**2) #print 'Force Magnitude: ', self.ft_mag self.wt_force_out.publish(self.ft_mag) def approach_to_contact(self, ps, overshoot=0.05): self.stop_maintain = True self.aul.update_frame(ps) appr = self.aul.find_approach(ps, 0.15) goal = self.aul.find_approach(ps, -overshoot) (appr_reachable, ik_goal) = self.aul.full_ik_check(appr) (goal_reachable, ik_goal) = self.aul.full_ik_check(goal) if appr_reachable and goal_reachable: traj = self.aul.build_trajectory(goal,appr,tot_points=600) prep = self.aul.move_arm_to(appr) if prep: curr_traj_point = self.advance_to_contact(traj) self.maintain_norm_force(traj, curr_traj_point) else: rospy.loginfo("Cannot reach desired 'move-to-contact' point") self.wt_log_out.publish(data="Cannot reach desired 'move-to-contact' point") def advance_to_contact(self, traj): completed = False curr_traj_point = 0 advance_rate = rospy.Rate(30) while not (rospy.is_shutdown() or completed): if not (curr_traj_point >= len(traj.points)): self.aul.send_traj_point(traj.points[curr_traj_point], 0.05) #hfm_pose = self.aul.hand_frame_move(0.003) #self.aul.blind_move(hfm_pose,0.9) advance_rate.sleep() curr_traj_point += 1 else: rospy.loginfo("Moved past expected contact, but no contact found!") self.wt_log_out.publish(data="Moved past expected contact, but no contact found!") completed = True if self.ft.wrench.force.z < -1.5: completed = True return curr_traj_point def maintain_norm_force(self, traj, curr_traj_point=0, mean=0, std=1): self.stop_maintain = False maintain_rate = rospy.Rate(100) while not (rospy.is_shutdown() or self.stop_maintain): if self.ft.wrench.force.z > mean + std: curr_traj_point += 1 if not (curr_traj_point >= len(traj.points)): self.aul.send_traj_point(traj.points[curr_traj_point], 0.033) rospy.sleep(0.033) else: rospy.loginfo("Force too low, but extent of the trajectory is reached") self.wt_log_out.publish(data="Force too low, but extent of the trajectory is reached") stopped = True elif self.ft.wrench.force.z < mean - std: curr_traj_point -= 1 if curr_traj_point >= 0: self.aul.send_traj_point(traj.points[curr_traj_point], 0.033) rospy.sleep(0.033) else: rospy.loginfo("Beginning of trajectory reached, cannot back away further") self.wt_log_out.publish(data="Beginning of trajectory reached, cannot back away further") stopped = True maintain_rate.sleep() mean = -self.force_goal_mean std = self.force_goal_std # def maintain_net_force(self, mean=0, std=3): # self.stop_maintain = False # maintain_rate = rospy.Rate(100) # while not (rospy.is_shutdown() or self.stop_maintain): # if self.ft_mag > mean + 8: # curr_angs = self.aul.joint_state_act.positions # try: # x_plus = np.subtract(self.aul.ik_pose_proxy(self.aul.form_ik_request(self.aul.hand_frame_move(0.02))).solution.joint_state.position, curr_angs) # x_minus = np.subtract(self.aul.ik_pose_proxy(self.aul.form_ik_request(self.aul.hand_frame_move(-0.02))).solution.joint_state.position, curr_angs) # y_plus = np.subtract(self.aul.ik_pose_proxy(self.aul.form_ik_request(self.aul.hand_frame_move(0, 0.02, 0))).solution.joint_state.position, curr_angs) # y_minus = np.subtract(self.aul.ik_pose_proxy(self.aul.form_ik_request(self.aul.hand_frame_move(0, -0.02, 0))).solution.joint_state.position, curr_angs) # z_plus = np.subtract(self.aul.ik_pose_proxy(self.aul.form_ik_request(self.aul.hand_frame_move(0, 0, 0.02))).solution.joint_state.position, curr_angs) # z_minus = np.subtract(self.aul.ik_pose_proxy(self.aul.form_ik_request(self.aul.hand_frame_move(0, 0, -0.02))).solution.joint_state.position, curr_angs) # #print 'x: ', x_plus,'\r\n', x_minus # #print 'y: ', y_plus,'\r\n', y_minus # #print 'z: ', z_plus,'\r\n', z_minus # ft_sum = self.ft_mag # parts = np.divide([self.ft.wrench.force.x, self.ft.wrench.force.y, self.ft.wrench.force.z], ft_sum) # print 'parts', parts # ends = [[x_plus,x_minus],[y_plus, y_minus],[z_plus,z_minus]] # side = [[0]*7]*3 # for i, part in enumerate(parts): # if part >=0: # side[i] = ends[i][0] # else: # side[i] = ends[i][1] # # ang_out = curr_angs # for i in range(3): # ang_out -= np.average(side, 0, parts) # except: # print 'Near Joint Limits!' # self.aul.send_joint_angles(ang_out) # # #print 'x: ', x_plus, x_minus # maintain_rate.sleep() # mean = self.force_goal_mean # std = self.force_goal_std def maintain_net_force(self, mean=0, std=8): self.stop_maintain = False maintain_rate = rospy.Rate(100) while not (rospy.is_shutdown() or self.stop_maintain): if self.ft_mag > mean + 3: #print 'Moving to avoid force' goal = PoseStamped() goal.header.frame_id = 'l_netft_frame' goal.header.stamp = rospy.Time(0) goal.pose.position.x = -np.clip(self.ft.wrench.force.x/2500, -0.1, 0.1) goal.pose.position.y = -np.clip(self.ft.wrench.force.y/2500, -0.1, 0.1) goal.pose.position.z = -np.clip(self.ft.wrench.force.z/2500, -0.1, 0.1)+0.052 goal = self.tfl.transformPose('/torso_lift_link', goal) goal.header.stamp = rospy.Time.now() goal.pose.orientation = self.aul.curr_pose().pose.orientation self.test_pose.publish(goal) self.aul.fast_move(goal, 0.2) rospy.sleep(0.05) maintain_rate.sleep() mean = self.force_goal_mean std = self.force_goal_std #def mannequin(self): #mannequin_rate=rospy.Rate(10) #while not rospy.is_shutdown(): #joints = np.add(np.array(self.aul.joint_state_act.positions), # np.clip(np.array(self.aul.joint_state_err.positions), -0.05, 0.05)) #joints = self.aul.joint_state_act.positions #print joints #raw_input('Review Joint Angles') #self.aul.send_joint_angles(joints,0.1) #mannequin_rate.sleep() def force_wipe_agg(self, ps): self.aul.update_frame(ps) rospy.sleep(0.1) pose = self.aul.find_approach(ps, 0) (goal_reachable, ik_goal) = self.aul.ik_check(pose) if goal_reachable: if not self.force_wipe_started: appr = self.aul.find_approach(ps, 0.20) (appr_reachable, ik_goal) = self.aul.ik_check(appr) self.test_pose.publish(appr) if appr_reachable: self.force_wipe_start = pose self.force_wipe_appr = appr self.force_wipe_started = True else: rospy.loginfo("Cannot reach approach point, please choose another") self.wt_log_out.publish(data="Cannot reach approach point, please choose another") self.say.publish(data="I cannot get to a safe approach for there, please choose another point") else: self.force_wipe(self.force_wipe_start, pose) self.force_wipe_started = False else: rospy.loginfo("Cannot reach wiping point, please choose another") self.wt_log_out.publish(data="Cannot reach wiping point, please choose another") self.say.publish(data="I cannot reach there, please choose another point") def force_wipe(self, ps_start, ps_finish, travel = 0.05): ps_start.header.stamp = rospy.Time.now() ps_finish.header.stamp = rospy.Time.now() ps_start_far = self.aul.hand_frame_move(travel, 0, 0, ps_start) ps_start_near = self.aul.hand_frame_move(-travel, 0, 0, ps_start) ps_finish_far = self.aul.hand_frame_move(travel, 0, 0, ps_finish) ps_finish_near = self.aul.hand_frame_move(-travel, 0, 0, ps_finish) n_points = int(math.ceil(self.aul.calc_dist(ps_start, ps_finish)*7000)) print 'n_points: ', n_points mid_traj = self.aul.build_trajectory(ps_finish, ps_start, tot_points=n_points) near_traj = self.aul.build_trajectory(ps_finish_near, ps_start_near, tot_points=n_points) far_traj = self.aul.build_trajectory(ps_finish_far, ps_start_far, tot_points=n_points) near_angles = [list(near_traj.points[i].positions) for i in range(n_points)] mid_angles = [list(mid_traj.points[i].positions) for i in range(n_points)] far_angles = [list(far_traj.points[i].positions) for i in range(n_points)] fmn_diff = np.abs(np.subtract(near_angles, far_angles)) fmn_max = np.max(fmn_diff, axis=0) print 'fmn_max: ', fmn_max if any(fmn_max >math.pi/2): rospy.loginfo("TOO LARGE OF AN ANGLE CHANGE ALONG GRADIENT, IK REGION PROBLEMS!") self.wt_log_out.publish(data="The path requested required large movements (IK Limits cause angle wrapping)") self.say.publish(data="Large motion detected, cancelling. Please try again.") return for i in range(7): n_max = max(np.abs(np.diff(near_angles,axis=0)[i])) m_max = max(np.abs(np.diff(mid_angles,axis=0)[i])) f_max = max(np.abs(np.diff(far_angles,axis=0)[i])) n_mean = 4*np.mean(np.abs(np.diff(near_angles,axis=0)[i])) m_mean = 4*np.mean(np.abs(np.diff(mid_angles,axis=0)[i])) f_mean = 4*np.mean(np.abs(np.diff(far_angles,axis=0)[i])) print 'near: max: ', n_max, 'mean: ', n_mean print 'mid: max: ', m_max, 'mean: ', m_mean print 'far: max: ', f_max, 'mean: ', f_mean if (n_max >= n_mean) or (m_max >= m_mean) or (f_max >= f_mean): rospy.logerr("TOO LARGE OF AN ANGLE CHANGE ALONG PATH, IK REGION PROBLEMS!") self.wt_log_out.publish(data="The path requested required large movements (IK Limits cause angle wrapping)") self.say.publish(data="Large motion detected, cancelling. Please try again.") return near_diff = np.subtract(near_angles, mid_angles).tolist() far_diff = np.subtract(far_angles, mid_angles).tolist() self.say.publish(data="Moving to approach point") prep = self.aul.move_arm_to(self.force_wipe_appr) if prep: print 'cur angles: ', self.aul.joint_state_act.positions, 'near angs: ', near_angles[0] print np.abs(np.subtract(self.aul.joint_state_act.positions, near_angles[0])) if np.max(np.abs(np.subtract(self.aul.joint_state_act.positions,near_angles[0])))>math.pi: self.say.publish(data="Adjusting for-arm roll") print "Evasive Action!" angles = list(self.aul.joint_state_act.positions) flex = angles[5] angles[5] = -0.1 self.aul.send_joint_angles(angles, 4) rospy.sleep(4) angles[4] = near_angles[0][4] self.aul.send_joint_angles(angles,6) rospy.sleep(6) angles[5] = flex self.aul.send_joint_angles(angles, 4) rospy.sleep(4) self.say.publish(data="Making Approach.") self.aul.send_joint_angles(np.add(mid_angles[0],near_diff[0]), 7.5) rospy.sleep(7) self.rezero_wrench.publish(data=True) rospy.sleep(1) wipe_rate = rospy.Rate(400) self.stop_maintain = False count = 0 lap = 0 max_laps = 4 mean = self.force_goal_mean std = self.force_goal_std bias = 1 self.say.publish(data="Wiping") while not (rospy.is_shutdown() or self.stop_maintain) and (count + 1 <= n_points) and (lap < max_laps): #print "mean: ", mean, "std: ", std, "force: ", self.ft_mag if self.ft_mag >= mean + std: # print 'Force too high!' bias += (self.ft_mag/3500) elif self.ft_mag < mean - std: # print 'Force too low' bias -= max(0.001,(self.ft_mag/3500)) else: # print 'Force Within Range' count += 1 bias = np.clip(bias, -1, 1) if bias > 0.: diff = near_diff[count] else: diff = far_diff[count] angles_out = np.add(mid_angles[count], np.multiply(abs(bias), diff)) self.aul.send_joint_angles(angles_out, 0.008) rospy.sleep(0.0075) mean = self.force_goal_mean std = self.force_goal_std if count + 1>= n_points: count = 0 mid_angles.reverse() near_diff.reverse() far_diff.reverse() lap += 1 #if lap == 3: # self.say.publish(data="Hold Still, you rascal!") rospy.sleep(0.5) self.say.publish(data="Pulling away") self.aul.send_joint_angles(near_angles[0],5) rospy.sleep(5) self.say.publish(data="Finished wiping. Thank you") def torso_frame_move(self, msg): self.stop_maintain = True goal = self.aul.curr_pose() goal.pose.position.x += msg.x goal.pose.position.y += msg.y goal.pose.position.z += msg.z self.aul.blind_move(goal) def hand_move(self, f32): self.stop_maintain = True hfm_pose = self.aul.hand_frame_move(f32.data) self.aul.blind_move(hfm_pose) def norm_approach(self, pose): self.stop_maintain = True self.aul.update_frame(pose) appr = self.aul.find_approach(pose, 0.15) self.aul.move_arm_to(appr) def grasp(self, ps): self.stop_maintain = True rospy.loginfo("Initiating Grasp Sequence") self.wt_log_out.publish(data="Initiating Grasp Sequence") self.aul.update_frame(ps) approach = self.aul.find_approach(ps, 0.15) rospy.loginfo("approach: \r\n %s" %approach) at_appr = self.aul.move_arm_to(approach) rospy.loginfo("arrived at approach: %s" %at_appr) if at_appr: opened = self.aul.gripper(0.09) if opened: rospy.loginfo("making linear approach") #hfm_pose = self.aul.hand_frame_move(0.23) hfm_pose = self.aul.find_approach(ps,-0.02) self.aul.blind_move(hfm_pose) self.aul.wait_for_stop(2) closed = self.aul.gripper(0.0) if not closed: rospy.loginfo("Couldn't close completely: Grasp likely successful") hfm_pose = self.aul.hand_frame_move(-0.23) self.aul.blind_move(hfm_pose) else: pass def prep_surf_wipe(self, point): pixel_u = point.x pixel_v = point.y test_pose = self.p23d_proxy(pixel_u, pixel_v).pixel3d self.aul.update_frame(test_pose) test_pose = self.aul.find_approach(test_pose, 0) (reachable, ik_goal) = self.aul.full_ik_check(test_pose) if reachable: if not self.surf_wipe_started: start_pose = test_pose self.surf_wipe_start = [pixel_u, pixel_v, start_pose] self.surf_wipe_started = True rospy.loginfo("Received valid starting position for wiping action") self.wt_log_out.publish(data="Received valid starting position for wiping action") return #Return after 1st point, wait for second else: rospy.loginfo("Received valid ending position for wiping action") self.wt_log_out.publish(data="Received valid ending position for wiping action") self.surf_wipe_started = False #Continue on successful 2nd point else: rospy.loginfo("Cannot reach wipe position, please try another") self.wt_log_out.publish(data="Cannot reach wipe position, please try another") return #Return on invalid point, wait for another dist = self.aul.calc_dist(self.surf_wipe_start[2],test_pose) print 'dist', dist num_points = dist/0.02 print 'num_points', num_points us = np.round(np.linspace(self.surf_wipe_start[0], pixel_u, num_points)) vs = np.round(np.linspace(self.surf_wipe_start[1], pixel_v, num_points)) surf_points = [PoseStamped() for i in xrange(len(us))] print "Surface Points", [us,vs] for i in xrange(len(us)): pose = self.p23d_proxy(us[i],vs[i]).pixel3d self.aul.update_frame(pose) surf_points[i] = self.aul.find_approach(pose,0) print i+1, '/', len(us) self.aul.blind_move(surf_points[0]) self.aul.wait_for_stop() for pose in surf_points: self.aul.blind_move(pose,2.5) rospy.sleep(2) #self.aul.wait_for_stop() self.aul.hand_frame_move(-0.1) def poke(self, ps): self.stop_maintain = True self.aul.update_frame(ps) appr = self.aul.find_approach(ps,0.15) touch = self.aul.find_approach(ps,0) prepared = self.aul.move_arm_to(appr) if prepared: self.aul.blind_move(touch) self.aul.wait_for_stop() rospy.sleep(7) self.aul.blind_move(appr) def swipe(self, ps): traj = self.prep_wipe(ps) if traj is not None: self.stop_maintain = True self.wipe_move(traj, 1) def wipe(self, ps): traj = self.prep_wipe(ps) if traj is not None: self.stop_maintain = True self.wipe_move(traj, 4) def prep_wipe(self, ps): #print "Prep Wipe Received: %s" %pa self.aul.update_frame(ps) print "Updating frame to: %s \r\n" %ps if not self.wipe_started: self.wipe_appr_seed = ps self.wipe_ends[0] = self.aul.find_approach(ps, 0) print "wipe_end[0]: %s" %self.wipe_ends[0] (reachable, ik_goal) = self.aul.full_ik_check(self.wipe_ends[0]) if not reachable: rospy.loginfo("Cannot find approach for initial wipe position, please try another") self.wt_log_out.publish(data="Cannot find approach for initial wipe position, please try another") return None else: self.wipe_started = True rospy.loginfo("Received starting position for wiping action") self.wt_log_out.publish(data="Received starting position for wiping action") return None else: self.wipe_ends[1] = self.aul.find_approach(ps, 0) self.wipe_ends.reverse() (reachable, ik_goal) = self.aul.full_ik_check(self.wipe_ends[1]) if not reachable: rospy.loginfo("Cannot find approach for final wipe position, please try another") self.wt_log_out.publish(data="Cannot find approach for final wipe position, please try another") return None else: rospy.loginfo("Received End position for wiping action") self.wt_log_out.publish(data="Received End position for wiping action") self.aul.update_frame(self.wipe_ends[0]) self.wipe_ends[1].header.stamp = rospy.Time.now() self.tfl.waitForTransform(self.wipe_ends[1].header.frame_id, 'lh_utility_frame', rospy.Time.now(), rospy.Duration(3.0)) fin_in_start = self.tfl.transformPose('lh_utility_frame', self.wipe_ends[1]) ang = math.atan2(-fin_in_start.pose.position.z, -fin_in_start.pose.position.y)+(math.pi/2) q_st_rot = transformations.quaternion_about_axis(ang, (1,0,0)) q_st_new = transformations.quaternion_multiply([self.wipe_ends[0].pose.orientation.x, self.wipe_ends[0].pose.orientation.y, self.wipe_ends[0].pose.orientation.z, self.wipe_ends[0].pose.orientation.w],q_st_rot) self.wipe_ends[0].pose.orientation.x = q_st_new[0] self.wipe_ends[0].pose.orientation.y = q_st_new[1] self.wipe_ends[0].pose.orientation.z = q_st_new[2] self.wipe_ends[0].pose.orientation.w = q_st_new[3] self.aul.update_frame(self.wipe_ends[1]) self.wipe_ends[0].header.stamp = rospy.Time.now() self.tfl.waitForTransform(self.wipe_ends[0].header.frame_id, 'lh_utility_frame', rospy.Time.now(), rospy.Duration(3.0)) start_in_fin = self.tfl.transformPose('lh_utility_frame', self.wipe_ends[0]) ang = math.atan2(start_in_fin.pose.position.z, start_in_fin.pose.position.y)+(math.pi/2) q_st_rot = transformations.quaternion_about_axis(ang, (1,0,0)) q_st_new = transformations.quaternion_multiply([self.wipe_ends[1].pose.orientation.x, self.wipe_ends[1].pose.orientation.y, self.wipe_ends[1].pose.orientation.z, self.wipe_ends[1].pose.orientation.w],q_st_rot) self.wipe_ends[1].pose.orientation.x = q_st_new[0] self.wipe_ends[1].pose.orientation.y = q_st_new[1] self.wipe_ends[1].pose.orientation.z = q_st_new[2] self.wipe_ends[1].pose.orientation.w = q_st_new[3] self.aul.update_frame(self.wipe_appr_seed) appr = self.aul.find_approach(self.wipe_appr_seed, 0.15) appr.pose.orientation = self.wipe_ends[1].pose.orientation prepared = self.aul.move_arm_to(appr) if prepared: #self.aul.advance_to_contact() self.aul.blind_move(self.wipe_ends[1]) traj = self.aul.build_trajectory(self.wipe_ends[1], self.wipe_ends[0]) wipe_traj = self.aul.build_follow_trajectory(traj) self.aul.wait_for_stop() self.wipe_started = False return wipe_traj #self.wipe(wipe_traj) else: rospy.loginfo("Failure reaching start point, please try again") self.wt_log_out.publish(data="Failure reaching start point, please try again") def wipe_move(self, traj_goal, passes=4): times = [] for i in range(len(traj_goal.trajectory.points)): times.append(traj_goal.trajectory.points[i].time_from_start) count = 0 while count < passes: traj_goal.trajectory.points.reverse() for i in range(len(times)): traj_goal.trajectory.points[i].time_from_start = times[i] traj_goal.trajectory.header.stamp = rospy.Time.now() assert traj_goal.trajectory.points[0] != [] self.aul.l_arm_follow_traj_client.send_goal(traj_goal) self.aul.l_arm_follow_traj_client.wait_for_result(rospy.Duration(20)) rospy.sleep(0.5)# Pause at end of swipe count += 1 rospy.loginfo("Done Wiping") self.wt_log_out.publish(data="Done Wiping") hfm_pose = self.aul.hand_frame_move(-0.15) self.aul.blind_move(hfm_pose) def broadcast_pose(self): broadcast_rate = rospy.Rate(10) while not rospy.is_shutdown(): self.pose_out.publish(self.aul.curr_pose()) broadcast_rate.sleep()
def tableposecallback(data): global count pub = rospy.Publisher('/cmd_vel', Twist, queue_size=1) if (not (math.isnan(data.tables[0].pose.orientation.x))): tf_listener = TransformListener() table_pose = data.tables[0].pose tpose = PoseStamped() tpose.header.frame_id = "/camera1_color_optical_frame" tpose.pose = table_pose tpose_in_base = tf_listener.transformPose("/base_link", p1) #rospy.loginfo(rospy.get_caller_id() + "I heard %s", math.isnan(data.tables[0].pose.orientation.x)) point_list = data.tables[0].convex_hull t_point_list = [] temp_point = PointStamped() for i in range(0, len(point_list)): temp_point.header.frame_id = "/camera_link_optical_1" temp_point.point.x = point_list[i].x temp_point.point.y = point_list[i].y temp_point.point.z = point_list[i].z t_point = tf_listener.transformPoint("/base_link", temp_point) t_point_list.append([point_list[i].x, point_list[i].y]) #remove outliers count = count + 1 xy_points = array(t_point_list) # Find convex hull hull_points = qhull2D(xy_points) # Find minimum area bounding rectangle (rot_angle, area, width, height, center_point, corner_points) = minBoundingRect(hull_points) # Verbose output of return data ''' print "Minimum area bounding box:" print "Rotation angle:", rot_angle, "rad (", rot_angle*(180/math.pi), "deg )" print "Width:", width, " Height:", height, " Area:", area print "Center point: \n", center_point # numpy array print "Corner points: \n", corner_points, "\n" # numpy array print("Table Pose wrt base link") print(p_in_base.pose.position.x) print(p_in_base.pose.position.y) print(p_in_base.pose.position.z) table_angle = rot_angle*(180/math.pi) triplePoints = [] marker = Marker() marker.header.frame_id = "/base_link"; marker.id = 1 marker.type = Marker.POINTS marker.action = Marker.ADD marker.pose.orientation.w = 1 marker.scale.x = 0.1 marker.scale.y = 0.1 marker.color.b = 1.0 marker.color.a = 1.0 #transform from x,y points to x,y,z points for i in range(0,len(corner_points)): p = Point() p.x = corner_points[i,0] p.y = corner_points[i,1] p.z = 1 marker.points.append(p) ''' twist = Twist() if (count >= 20): count = 1 calculated_time = abs(p_in_base.pose.position.y) / 0.1 print(calculated_time) y_corr_start_time = time.time() print(y_corr_start_time) while ((time.time() - y_corr_start_time) < calculated_time): #print("Moving in y") twist.linear.x = 0 if (p_in_base.pose.position.y < 0): twist.linear.y = -0.1 else: twist.linear.y = 0.1 twist.linear.z = 0 twist.angular.x = 0 twist.angular.y = 0 twist.angular.z = 0 pub.publish(twist) calculated_time = (p_in_base.pose.position.x - 0.85) / 0.1 x_corr_start_time = time.time() while ((time.time() - x_corr_start_time) < calculated_time): #print("Moving in x") twist.linear.x = 0.1 twist.linear.y = 0 twist.linear.z = 0 twist.angular.x = 0 twist.angular.y = 0 twist.angular.z = 0 pub.publish(twist) if (table_angle > 45): #print("Rotate robot clockwise") #print("Angle",(90-table_angle)) calculated_time = (abs(90 - table_angle)) * (math.pi / 180) / 0.2 th_corr_start_time = time.time() while ((time.time() - th_corr_start_time) < calculated_time): twist.linear.x = 0 twist.linear.y = 0 twist.linear.z = 0 twist.angular.x = 0 twist.angular.y = 0 twist.angular.z = -0.1 pub.publish(twist) else: #print("Rotate robot anticlockwise") #print("Angle",(table_angle)) calculated_time = (abs(table_angle)) * (math.pi / 180) / 0.2 th_corr_start_time = time.time() while ((time.time() - th_corr_start_time) < calculated_time): twist.linear.x = 0 twist.linear.y = 0 twist.linear.z = 0 twist.angular.x = 0 twist.angular.y = 0 twist.angular.z = 0.1 pub.publish(twist) print("Robot pose corrected\n")
class ParticleFilter: """ The class that represents a Particle Filter ROS Node Attributes list: initialized: a Boolean flag to communicate to other class methods that initializaiton is complete base_frame: the name of the robot base coordinate frame (should be "base_link" for most robots) map_frame: the name of the map coordinate frame (should be "map" in most cases) odom_frame: the name of the odometry coordinate frame (should be "odom" in most cases) scan_topic: the name of the scan topic to listen to (should be "scan" in most cases) n_particles: the number of particles in the filter d_thresh: the amount of linear movement before triggering a filter update a_thresh: the amount of angular movement before triggering a filter update laser_max_distance: the maximum distance to an obstacle we should use in a likelihood calculation pose_listener: a subscriber that listens for new approximate pose estimates (i.e. generated through the rviz GUI) particle_pub: a publisher for the particle cloud laser_subscriber: listens for new scan data on topic self.scan_topic tf_listener: listener for coordinate transforms tf_broadcaster: broadcaster for coordinate transforms particle_cloud: a list of particles representing a probability distribution over robot poses current_odom_xy_theta: the pose of the robot in the odometry frame when the last filter update was performed. The pose is expressed as a list [x,y,theta] (where theta is the yaw) map: the map we will be localizing ourselves in. The map should be of type nav_msgs/OccupancyGrid """ def __init__(self): self.initialized = False # make sure we don't perform updates before everything is setup rospy.init_node( 'pf') # tell roscore that we are creating a new node named "pf" self.base_frame = "base_link" # the frame of the robot base self.map_frame = "map" # the name of the map coordinate frame self.odom_frame = "odom" # the name of the odometry coordinate frame self.scan_topic = "scan" # the topic where we will get laser scans from self.n_particles = 300 # the number of particles to use self.d_thresh = 0.2 # the amount of linear movement before performing an update self.a_thresh = math.pi / 6 # the amount of angular movement before performing an update self.laser_max_distance = 2.0 # maximum penalty to assess in the likelihood field model # TODO: define additional constants if needed # Setup pubs and subs # pose_listener responds to selection of a new approximate robot location (for instance using rviz) rospy.Subscriber("initialpose", PoseWithCovarianceStamped, self.update_initial_pose) # publish the current particle cloud. This enables viewing particles in rviz. self.particle_pub = rospy.Publisher("particlecloud", PoseArray, queue_size=10) # laser_subscriber listens for data from the lidar rospy.Subscriber(self.scan_topic, LaserScan, self.scan_received) # enable listening for and broadcasting coordinate transforms self.tf_listener = TransformListener() self.tf_broadcaster = TransformBroadcaster() self.particle_cloud = [] # change use_projected_stable_scan to True to use point clouds instead of laser scans self.use_projected_stable_scan = False self.last_projected_stable_scan = None if self.use_projected_stable_scan: # subscriber to the odom point cloud rospy.Subscriber("projected_stable_scan", PointCloud, self.projected_scan_received) self.current_odom_xy_theta = [] self.occupancy_field = OccupancyField() self.transform_helper = TFHelper() self.initialized = True def update_robot_pose(self, timestamp): """ Update the estimate of the robot's pose given the updated particles. There are two logical methods for this: (1): compute the mean pose (2): compute the most likely pose (i.e. the mode of the distribution) """ # first make sure that the particle weights are normalized self.normalize_particles() # TODO: assign the latest pose into self.robot_pose as a geometry_msgs.Pose object # just to get started we will fix the robot's pose to always be at the origin self.robot_pose = Pose() self.transform_helper.fix_map_to_odom_transform( self.robot_pose, timestamp) def projected_scan_received(self, msg): self.last_projected_stable_scan = msg def update_particles_with_odom(self, msg): """ Update the particles using the newly given odometry pose. The function computes the value delta which is a tuple (x,y,theta) that indicates the change in position and angle between the odometry when the particles were last updated and the current odometry. msg: this is not really needed to implement this, but is here just in case. """ new_odom_xy_theta = self.transform_helper.convert_pose_to_xy_and_theta( self.odom_pose.pose) # compute the change in x,y,theta since our last update if self.current_odom_xy_theta: old_odom_xy_theta = self.current_odom_xy_theta delta = (new_odom_xy_theta[0] - self.current_odom_xy_theta[0], new_odom_xy_theta[1] - self.current_odom_xy_theta[1], new_odom_xy_theta[2] - self.current_odom_xy_theta[2]) self.current_odom_xy_theta = new_odom_xy_theta else: self.current_odom_xy_theta = new_odom_xy_theta return # TODO: modify particles using delta def map_calc_range(self, x, y, theta): """ Difficulty Level 3: implement a ray tracing likelihood model... Let me know if you are interested """ # TODO: nothing unless you want to try this alternate likelihood model pass def resample_particles(self): """ Resample the particles according to the new particle weights. The weights stored with each particle should define the probability that a particular particle is selected in the resampling step. You may want to make use of the given helper function draw_random_sample. """ # make sure the distribution is normalized self.normalize_particles() # TODO: fill out the rest of the implementation def update_particles_with_laser(self, msg): """ Updates the particle weights in response to the scan contained in the msg """ # TODO: implement this pass @staticmethod def draw_random_sample(choices, probabilities, n): """ Return a random sample of n elements from the set choices with the specified probabilities choices: the values to sample from represented as a list probabilities: the probability of selecting each element in choices represented as a list n: the number of samples """ values = np.array(range(len(choices))) probs = np.array(probabilities) bins = np.add.accumulate(probs) inds = values[np.digitize(random_sample(n), bins)] samples = [] for i in inds: samples.append(deepcopy(choices[int(i)])) return samples def update_initial_pose(self, msg): """ Callback function to handle re-initializing the particle filter based on a pose estimate. These pose estimates could be generated by another ROS Node or could come from the rviz GUI """ xy_theta = self.transform_helper.convert_pose_to_xy_and_theta( msg.pose.pose) self.initialize_particle_cloud(msg.header.stamp, xy_theta) def initialize_particle_cloud(self, timestamp, xy_theta=None): """ Initialize the particle cloud. Arguments xy_theta: a triple consisting of the mean x, y, and theta (yaw) to initialize the particle cloud around. If this input is omitted, the odometry will be used """ if xy_theta is None: xy_theta = self.transform_helper.convert_pose_to_xy_and_theta( self.odom_pose.pose) self.particle_cloud = [] # TODO create particles self.normalize_particles() self.update_robot_pose(timestamp) def normalize_particles(self): """ Make sure the particle weights define a valid distribution (i.e. sum to 1.0) """ # TODO: implement this pass def publish_particles(self, msg): particles_conv = [] for p in self.particle_cloud: particles_conv.append(p.as_pose()) # actually send the message so that we can view it in rviz self.particle_pub.publish( PoseArray(header=Header(stamp=rospy.Time.now(), frame_id=self.map_frame), poses=particles_conv)) def scan_received(self, msg): """ This is the default logic for what to do when processing scan data. Feel free to modify this, however, we hope it will provide a good guide. The input msg is an object of type sensor_msgs/LaserScan """ if not (self.initialized): # wait for initialization to complete return if not (self.tf_listener.canTransform( self.base_frame, msg.header.frame_id, msg.header.stamp)): # need to know how to transform the laser to the base frame # this will be given by either Gazebo or neato_node return # wait a little while to see if the transform becomes available. This fixes a race # condition where the scan would arrive a little bit before the odom to base_link transform # was updated. self.tf_listener.waitForTransform(self.base_frame, self.odom_frame, msg.header.stamp, rospy.Duration(0.5)) if not (self.tf_listener.canTransform(self.base_frame, self.odom_frame, msg.header.stamp)): # need to know how to transform between base and odometric frames # this will eventually be published by either Gazebo or neato_node return # calculate pose of laser relative to the robot base p = PoseStamped( header=Header(stamp=rospy.Time(0), frame_id=msg.header.frame_id)) self.laser_pose = self.tf_listener.transformPose(self.base_frame, p) # find out where the robot thinks it is based on its odometry p = PoseStamped(header=Header(stamp=msg.header.stamp, frame_id=self.base_frame), pose=Pose()) self.odom_pose = self.tf_listener.transformPose(self.odom_frame, p) # store the the odometry pose in a more convenient format (x,y,theta) new_odom_xy_theta = self.transform_helper.convert_pose_to_xy_and_theta( self.odom_pose.pose) if not self.current_odom_xy_theta: self.current_odom_xy_theta = new_odom_xy_theta return if not (self.particle_cloud): # now that we have all of the necessary transforms we can update the particle cloud self.initialize_particle_cloud(msg.header.stamp) elif (math.fabs(new_odom_xy_theta[0] - self.current_odom_xy_theta[0]) > self.d_thresh or math.fabs(new_odom_xy_theta[1] - self.current_odom_xy_theta[1]) > self.d_thresh or math.fabs(new_odom_xy_theta[2] - self.current_odom_xy_theta[2]) > self.a_thresh): # we have moved far enough to do an update! self.update_particles_with_odom(msg) # update based on odometry if self.last_projected_stable_scan: last_projected_scan_timeshift = deepcopy( self.last_projected_stable_scan) last_projected_scan_timeshift.header.stamp = msg.header.stamp self.scan_in_base_link = self.tf_listener.transformPointCloud( "base_link", last_projected_scan_timeshift) self.update_particles_with_laser(msg) # update based on laser scan self.update_robot_pose(msg.header.stamp) # update robot's pose self.resample_particles( ) # resample particles to focus on areas of high density # publish particles (so things like rviz can see them) self.publish_particles(msg)
class ParticleFilter: """ The class that represents a Particle Filter ROS Node Attributes list: initialized: a Boolean flag to communicate to other class methods that initializaiton is complete base_frame: the name of the robot base coordinate frame (should be "base_link" for most robots) map_frame: the name of the map coordinate frame (should be "map" in most cases) odom_frame: the name of the odometry coordinate frame (should be "odom" in most cases) scan_topic: the name of the scan topic to listen to (should be "scan" in most cases) n_particles: the number of particles in the filter d_thresh: the amount of linear movement before triggering a filter update a_thresh: the amount of angular movement before triggering a filter update laser_max_distance: the maximum distance to an obstacle we should use in a likelihood calculation pose_listener: a subscriber that listens for new approximate pose estimates (i.e. generated through the rviz GUI) particle_pub: a publisher for the particle cloud laser_subscriber: listens for new scan data on topic self.scan_topic tf_listener: listener for coordinate transforms tf_broadcaster: broadcaster for coordinate transforms particle_cloud: a list of particles representing a probability distribution over robot poses current_odom_xy_theta: the pose of the robot in the odometry frame when the last filter update was performed. The pose is expressed as a list [x,y,theta] (where theta is the yaw) map: the map we will be localizing ourselves in. The map should be of type nav_msgs/OccupancyGrid """ def __init__(self): self.initialized = False # make sure we don't perform updates before everything is setup rospy.init_node('pf') # tell roscore that we are creating a new node named "pf" self.base_frame = "base_link" # the frame of the robot base self.map_frame = "map" # the name of the map coordinate frame self.odom_frame = "odom" # the name of the odometry coordinate frame self.scan_topic = "scan" # the topic where we will get laser scans from self.n_particles = 100 # the number of particles to use self.d_thresh = 0.2 # the amount of linear movement before performing an update self.a_thresh = math.pi/6 # the amount of angular movement before performing an update self.laser_max_distance = 2.0 # maximum penalty to assess in the likelihood field model self.sigma = 0.08 # TODO: define additional constants if needed # Setup pubs and subs # pose_listener responds to selection of a new approximate robot location (for instance using rviz) self.pose_listener = rospy.Subscriber("initialpose", PoseWithCovarianceStamped, self.update_initial_pose) # publish the current particle cloud. This enables viewing particles in rviz. self.particle_pub = rospy.Publisher("particlecloud", PoseArray, queue_size=10) self.marker_pub = rospy.Publisher("markers", MarkerArray, queue_size=10) # laser_subscriber listens for data from the lidar # Dados do Laser: Mapa de verossimilhança/Occupancy field/Likehood map e Traçado de raios. # Traçado de raios: Leitura em ângulo que devolve distância, através do sensor. Dado o mapa, # extender a direção da distância pra achar um obstáculo. self.laser_subscriber = rospy.Subscriber(self.scan_topic, LaserScan, self.scan_received) # enable listening for and broadcasting coordinate transforms #atualização de posição(odometria) self.tf_listener = TransformListener() self.tf_broadcaster = TransformBroadcaster() self.particle_cloud = [] self.current_odom_xy_theta = [] #Chamar o mapa a partir de funcao externa mapa = chama_mapa.obter_mapa() # request the map from the map server, the map should be of type nav_msgs/OccupancyGrid # TODO: fill in the appropriate service call here. The resultant map should be assigned be passed # into the init method for OccupancyField # for now we have commented out the occupancy field initialization until you can successfully fetch the map self.occupancy_field = OccupancyField(mapa) self.initialized = True def update_robot_pose(self): print("Update Robot Pose") """ Update the estimate of the robot's pose given the updated particles. There are two logical methods for this: (1): compute the mean pose (2): compute the most likely pose (i.e. the mode of the distribution) """ # first make sure that the particle weights are normalized self.normalize_particles() # TODO: assign the lastest pose into self.robot_pose as a geometry_msgs.Pose object # just to get started we will fix the robot's pose to always be at the origin #Variaveis para soma do X,Y e angulo Theta da particula x_sum = 0 y_sum = 0 theta_sum = 0 #Loop de soma para as variaveis relativas a probabilidade da particula for p in self.particle_cloud: x_sum += p.x * p.w y_sum += p.y * p.w theta_sum += p.theta * p.w #Atributo robot_pose eh o pose da melhor particula possivel a partir das variaveis de soma self.robot_pose = Particle(x=x_sum, y=y_sum, theta=theta_sum).as_pose() def update_particles_with_odom(self,msg): print("Update Particles with Odom") """ Update the particles using the newly given odometry pose. The function computes the value delta which is a tuple (x,y,theta) that indicates the change in position and angle between the odometry when the particles were last updated and the current odometry. msg: this is not really needed to implement this, but is here just in case. """ new_odom_xy_theta = convert_pose_to_xy_and_theta(self.odom_pose.pose) # compute the change in x,y,theta since our last update if self.current_odom_xy_theta: old_odom_xy_theta = self.current_odom_xy_theta delta = (new_odom_xy_theta[0] - self.current_odom_xy_theta[0], new_odom_xy_theta[1] - self.current_odom_xy_theta[1], new_odom_xy_theta[2] - self.current_odom_xy_theta[2]) self.current_odom_xy_theta = new_odom_xy_theta else: self.current_odom_xy_theta = new_odom_xy_theta #R eh o raio feito a partir de um pitagoras com o X e Y da variacao Delta r = math.sqrt(delta[0]**2 + delta[1]**2) #Funcao para conseguir um valor entre -pi e pi e subtrair o antigo valor de theta. (Achei um pouco confusa, ) phi = math.atan2(delta[1], delta[0])-old_odom_xy_theta[2] for particle in self.particle_cloud: particle.x += r*math.cos(phi+particle.theta) particle.y += r*math.sin(phi+particle.theta) particle.theta += delta[2] # TODO: modify particles using delta # For added difficulty: Implement sample_motion_odometry (Prob Rob p 136) def map_calc_range(self,x,y,theta): """ Difficulty Level 3: implement a ray tracing likelihood model... Let me know if you are interested """ # TODO: nothing unless you want to try this alternate likelihood model pass def resample_particles(self): """ Resample the particles according to the new particle weights. The weights stored with each particle should define the probability that a particular particle is selected in the resampling step. You may want to make use of the given helper function draw_random_sample. """ #Primeiro de tudo, normalizar particulas self.normalize_particles() #Criar array do numpy vazia do tamanho do numero de particulas. values = np.empty(self.n_particles) #Preencher essa lista com os indices das particulas for i in range(self.n_particles): values[i] = i #Criar uma lista para novas particulas new_particles = [] #Criar lista com os indices das particulas com mais probabilidade random_particles = ParticleFilter.weighted_values(values,[p.w for p in self.particle_cloud],self.n_particles) for i in random_particles: #Transformar o I em inteiro para corrigir bug de float int_i = int(i) #Pegar particula na possicao I na nuvem de particulas. p = self.particle_cloud[int_i] #Adicionar particulas somando um valor aleatorio da distribuicao gauss com media = 0 e desvio padrao = 0.025 new_particles.append(Particle(x=p.x+gauss(0,.025),y=p.y+gauss(0,.025),theta=p.theta+gauss(0,.025))) #Igualar nuvem de particulas a novo sample criado self.particle_cloud = new_particles #Normalizar mais uma vez as particulas. self.normalize_particles() def update_particles_with_laser(self, msg): print("Update Particles with laser") """ Updates the particle weights in response to the scan contained in the msg """ for p in self.particle_cloud: for i in range(360): #Distancia no angulo I distancia = msg.ranges[i] x = distancia * math.cos(i + p.theta) y = distancia * math.sin(i + p.theta) #Verificar se distancia minima eh diferente de nan erro_nan = self.occupancy_field.get_closest_obstacle_distance(x,y) if erro_nan is not float('nan'): # Adicionar valor para corrigir erro de nan (Retirado de outro codigo) p.w += math.exp(-erro_nan*erro_nan/(2*self.sigma**2)) #Normalizar particulas e criar um novo sample das mesmas self.normalize_particles() self.resample_particles() @staticmethod def weighted_values(values, probabilities, size): """ Return a random sample of size elements from the set values with the specified probabilities values: the values to sample from (numpy.ndarray) probabilities: the probability of selecting each element in values (numpy.ndarray) size: the number of samples """ bins = np.add.accumulate(probabilities) return values[np.digitize(random_sample(size), bins)] @staticmethod #Nao estou usando esse metodo. Apenas o weighted_values def draw_random_sample(choices, probabilities, n): print("Draw Random Sample") """ Return a random sample of n elements from the set choices with the specified probabilities choices: the values to sample from represented as a list probabilities: the probability of selecting each element in choices represented as a list n: the number of samples """ values = np.array(range(len(choices))) probs = np.array(probabilities) bins = np.add.accumulate(probs) inds = values[np.digitize(random_sample(n), bins)] samples = [] for i in inds: samples.append(deepcopy(choices[int(i)])) return samples def update_initial_pose(self, msg): print("Update Initial Pose") """ Callback function to handle re-initializing the particle filter based on a pose estimate. These pose estimates could be generated by another ROS Node or could come from the rviz GUI """ xy_theta = convert_pose_to_xy_and_theta(msg.pose.pose) self.initialize_particle_cloud(xy_theta) self.fix_map_to_odom_transform(msg) def initialize_particle_cloud(self, xy_theta=None): """ Initialize the particle cloud. Arguments xy_theta: a triple consisting of the mean x, y, and theta (yaw) to initialize the particle cloud around. If this input is ommitted, the odometry will be used """ if xy_theta == None: xy_theta = convert_pose_to_xy_and_theta(self.odom_pose.pose) self.particle_cloud = [] # TODO create particles # Criando particula print("Inicializacao da Cloud de Particulas") #Valor auxiliar para nao dar erro na hora de criacao das particulas. Irrelevante valor_aux = 0.3 for i in range (self.n_particles): self.particle_cloud.append(Particle(0, 0, 0, valor_aux)) # Randomizar particulas em volta do robo. for i in self.particle_cloud: i.x = xy_theta[0]+ random.uniform(-1,1) i.y = xy_theta[1]+ random.uniform(-1,1) i.theta = xy_theta[2]+ random.uniform(-45,45) #Normalizar as particulas e dar update na posicao do robo self.normalize_particles() self.update_robot_pose() print(xy_theta) def normalize_particles(self): """ Make sure the particle weights define a valid distribution (i.e. sum to 1.0) """ #Soma total das probabilidades das particulas w_sum = sum([p.w for p in self.particle_cloud]) #Dividir cada probabilidade de uma particula pela soma total for particle in self.particle_cloud: particle.w /= w_sum # TODO: implement this def publish_particles(self, msg): print("Publicar Particulas.") print(len(self.particle_cloud)) particles_conv = [] for p in self.particle_cloud: particles_conv.append(p.as_pose()) # actually send the message so that we can view it in rviz self.particle_pub.publish(PoseArray(header=Header(stamp=rospy.Time.now(), frame_id=self.map_frame), poses=particles_conv)) def scan_received(self, msg): """ This is the default logic for what to do when processing scan data. Feel free to modify this, however, I hope it will provide a good guide. The input msg is an object of type sensor_msgs/LaserScan """ if not(self.initialized): print("Not Initialized") # wait for initialization to complete return if not(self.tf_listener.canTransform(self.base_frame,msg.header.frame_id,rospy.Time(0))): print("Not 2") # need to know how to transform the laser to the base frame # this will be given by either Gazebo or neato_node return if not(self.tf_listener.canTransform(self.base_frame,self.odom_frame,rospy.Time(0))): print("Not 3") # need to know how to transform between base and odometric frames # this will eventually be published by either Gazebo or neato_node return # calculate pose of laser relative ot the robot base p = PoseStamped(header=Header(stamp=rospy.Time(0), frame_id=msg.header.frame_id)) self.laser_pose = self.tf_listener.transformPose(self.base_frame,p) # find out where the robot thinks it is based on its odometry p = PoseStamped(header=Header(stamp = rospy.Time(0), frame_id=self.base_frame), pose=Pose()) self.odom_pose = self.tf_listener.transformPose(self.odom_frame, p) # store the the odometry pose in a more convenient format (x,y,theta) new_odom_xy_theta = convert_pose_to_xy_and_theta(self.odom_pose.pose) print("PASSOU") if not(self.particle_cloud): # now that we have all of the necessary transforms we can update the particle cloud self.initialize_particle_cloud() # cache the last odometric pose so we can only update our particle filter if we move more than self.d_thresh or self.a_thresh self.current_odom_xy_theta = new_odom_xy_theta # update our map to odom transform now that the particles are initialized self.fix_map_to_odom_transform(msg) elif (math.fabs(new_odom_xy_theta[0] - self.current_odom_xy_theta[0]) > self.d_thresh or math.fabs(new_odom_xy_theta[1] - self.current_odom_xy_theta[1]) > self.d_thresh or math.fabs(new_odom_xy_theta[2] - self.current_odom_xy_theta[2]) > self.a_thresh): # we have moved far enough to do an update! self.update_particles_with_odom(msg) # update based on odometry self.update_particles_with_laser(msg) # update based on laser scan self.update_robot_pose() # update robot's pose self.resample_particles() # resample particles to focus on areas of high density self.fix_map_to_odom_transform(msg) # update map to odom transform now that we have new particles # publish particles (so things like rviz can see them) self.publish_particles(msg) # direcionar particulas quando um obstaculo é identificado. def fix_map_to_odom_transform(self, msg): print("Fix Map to Odom Transform") """ This method constantly updates the offset of the map and odometry coordinate systems based on the latest results from the localizer """ (translation, rotation) = convert_pose_inverse_transform(self.robot_pose) p = PoseStamped(pose=convert_translation_rotation_to_pose(translation,rotation), header=Header(stamp=rospy.Time(0),frame_id=self.base_frame)) self.odom_to_map = self.tf_listener.transformPose(self.odom_frame, p) (self.translation, self.rotation) = convert_pose_inverse_transform(self.odom_to_map.pose) def broadcast_last_transform(self): print("Broadcast") """ Make sure that we are always broadcasting the last map to odom transformation. This is necessary so things like move_base can work properly. """ if not(hasattr(self,'translation') and hasattr(self,'rotation')): return self.tf_broadcaster.sendTransform(self.translation, self.rotation, rospy.get_rostime(), self.odom_frame, self.map_frame)
class MarkerProcessor(object): def __init__(self, use_dummy_transform=False): rospy.init_node('star_center_positioning_node') if use_dummy_transform: self.odom_frame_name = ROBOT_NAME+"_odom_dummy" else: self.odom_frame_name = ROBOT_NAME+"_odom" self.marker_locators = {} self.add_marker_locator(MarkerLocator(0,(-6*12*2.54/100.0,0),0)) self.add_marker_locator(MarkerLocator(1,(0.0,0.0),pi)) self.add_marker_locator(MarkerLocator(2,(0.0,10*12*2.54/100.0),0)) self.add_marker_locator(MarkerLocator(3,(-6*12*2.54/100.0,6*12*2.54/100.0),0)) self.add_marker_locator(MarkerLocator(4,(0.0,6*12*2.54/100.0),pi)) self.pose_correction = rospy.get_param('~pose_correction',0.0) self.phase_offset = rospy.get_param('~phase_offset',0.0) self.is_flipped = rospy.get_param('~is_flipped',False) srv = Server(STARPoseConfig, self.config_callback) self.marker_sub = rospy.Subscriber("ar_pose_marker", ARMarkers, self.process_markers) self.odom_sub = rospy.Subscriber(ROBOT_NAME+"/odom", Odometry, self.process_odom, queue_size=10) self.star_pose_pub = rospy.Publisher(ROBOT_NAME+"/STAR_pose",PoseStamped,queue_size=10) self.continuous_pose = rospy.Publisher(ROBOT_NAME+"/STAR_pose_continuous",PoseStamped,queue_size=10) self.tf_listener = TransformListener() self.tf_broadcaster = TransformBroadcaster() def add_marker_locator(self, marker_locator): self.marker_locators[marker_locator.id] = marker_locator def config_callback(self, config, level): self.phase_offset = config.phase_offset self.pose_correction = config.pose_correction return config def process_odom(self, msg): p = PoseStamped(header=Header(stamp=rospy.Time(0), frame_id=self.odom_frame_name), pose=msg.pose.pose) try: STAR_pose = self.tf_listener.transformPose("STAR", p) STAR_pose.header.stamp = msg.header.stamp self.continuous_pose.publish(STAR_pose) except Exception as inst: print "error is", inst def process_markers(self, msg): for marker in msg.markers: # do some filtering basd on prior knowledge # we know the approximate z coordinate and that all angles but yaw should be close to zero euler_angles = euler_from_quaternion((marker.pose.pose.orientation.x, marker.pose.pose.orientation.y, marker.pose.pose.orientation.z, marker.pose.pose.orientation.w)) angle_diffs = TransformHelpers.angle_diff(euler_angles[0],pi), TransformHelpers.angle_diff(euler_angles[1],0) print angle_diffs, marker.pose.pose.position.z if (marker.id in self.marker_locators and 3.0 <= marker.pose.pose.position.z <= 3.6 and fabs(angle_diffs[0]) <= .4 and fabs(angle_diffs[1]) <= .4): print "FOUND IT!" locator = self.marker_locators[marker.id] xy_yaw = list(locator.get_camera_position(marker)) if self.is_flipped: print "WE ARE FLIPPED!!!" xy_yaw[2] += pi print self.pose_correction print self.phase_offset xy_yaw[0] += self.pose_correction*cos(xy_yaw[2]+self.phase_offset) xy_yaw[1] += self.pose_correction*sin(xy_yaw[2]+self.phase_offset) orientation_tuple = quaternion_from_euler(0,0,xy_yaw[2]) pose = Pose(position=Point(x=-xy_yaw[0],y=-xy_yaw[1],z=0), orientation=Quaternion(x=orientation_tuple[0], y=orientation_tuple[1], z=orientation_tuple[2], w=orientation_tuple[3])) # TODO: use markers timestamp instead of now() (unfortunately, not populated currently by ar_pose) pose_stamped = PoseStamped(header=Header(stamp=msg.header.stamp,frame_id="STAR"),pose=pose) # TODO: use frame timestamp instead of now() self.star_pose_pub.publish(pose_stamped) self.fix_STAR_to_odom_transform(pose_stamped) def fix_STAR_to_odom_transform(self, msg): """ Super tricky code to properly update map to odom transform... do not modify this... Difficulty level infinity. """ (translation, rotation) = TransformHelpers.convert_pose_inverse_transform(msg.pose) p = PoseStamped(pose=TransformHelpers.convert_translation_rotation_to_pose(translation,rotation),header=Header(stamp=rospy.Time(),frame_id=ROBOT_NAME+"_base_link")) try: self.tf_listener.waitForTransform(ROBOT_NAME+"_odom",ROBOT_NAME+"_base_link",rospy.Time(),rospy.Duration(1.0)) except Exception as inst: print "whoops", inst return print "got transform" self.odom_to_STAR = self.tf_listener.transformPose(ROBOT_NAME+"_odom", p) (self.translation, self.rotation) = TransformHelpers.convert_pose_inverse_transform(self.odom_to_STAR.pose) def broadcast_last_transform(self): """ Make sure that we are always broadcasting the last map to odom transformation. This is necessary so things like move_base can work properly. """ if not(hasattr(self,'translation') and hasattr(self,'rotation')): return self.tf_broadcaster.sendTransform(self.translation, self.rotation, rospy.get_rostime(), self.odom_frame_name, "STAR") def run(self): r = rospy.Rate(10) while not rospy.is_shutdown(): self.broadcast_last_transform() r.sleep()
class ParticleFilter: """ The class that represents a Particle Filter ROS Node Attributes list: initialized: a Boolean flag to communicate to other class methods that initializaiton is complete base_frame: the name of the robot base coordinate frame (should be "base_link" for most robots) map_frame: the name of the map coordinate frame (should be "map" in most cases) odom_frame: the name of the odometry coordinate frame (should be "odom" in most cases) scan_topic: the name of the scan topic to listen to (should be "scan" in most cases) n_particles: the number of particles in the filter d_thresh: the amount of linear movement before triggering a filter update a_thresh: the amount of angular movement before triggering a filter update laser_max_distance: the maximum distance to an obstacle we should use in a likelihood calculation pose_listener: a subscriber that listens for new approximate pose estimates (i.e. generated through the rviz GUI) particle_pub: a publisher for the particle cloud laser_subscriber: listens for new scan data on topic self.scan_topic tf_listener: listener for coordinate transforms tf_broadcaster: broadcaster for coordinate transforms particle_cloud: a list of particles representing a probability distribution over robot poses current_odom_xy_theta: the pose of the robot in the odometry frame when the last filter update was performed. The pose is expressed as a list [x,y,theta] (where theta is the yaw) map: the map we will be localizing ourselves in. The map should be of type nav_msgs/OccupancyGrid """ def __init__(self): self.initialized = False # make sure we don't perform updates before everything is setup rospy.init_node('pf') # tell roscore that we are creating a new node named "pf" self.base_frame = "base_link" # the frame of the robot base self.map_frame = "map" # the name of the map coordinate frame self.odom_frame = "odom" # the name of the odometry coordinate frame self.scan_topic = "scan" # the topic where we will get laser scans from self.n_particles = 300 # the number of particles to use self.d_thresh = 0.2 # the amount of linear movement before performing an update self.a_thresh = math.pi/6 # the amount of angular movement before performing an update self.laser_max_distance = 2.0 # maximum penalty to assess in the likelihood field model self.robot_pose # TODO: define additional constants if needed # Setup pubs and subs # pose_listener responds to selection of a new approximate robot location (for instance using rviz) self.pose_listener = rospy.Subscriber("initialpose", PoseWithCovarianceStamped, self.update_initial_pose) # publish the current particle cloud. This enables viewing particles in rviz. self.particle_pub = rospy.Publisher("particlecloud", PoseArray) # laser_subscriber listens for data from the lidar self.laser_subscriber = rospy.Subscriber(self.scan_topic, LaserScan, self.scan_received) # enable listening for and broadcasting coordinate transforms self.tf_listener = TransformListener() self.tf_broadcaster = TransformBroadcaster() self.particle_cloud = [] self.current_odom_xy_theta = [] # request the map from the map server, the map should be of type nav_msgs/OccupancyGrid # TODO: fill in the appropriate service call here. The resultant map should be assigned be passed # into the init method for OccupancyField self.occupancy_field = OccupancyField(map) self.initialized = True def update_robot_pose(self): """ Update the estimate of the robot's pose given the updated particles. There are two logical methods for this: (1): compute the mean pose (level 2) (2): compute the most likely pose (i.e. the mode of the distribution) (level 1) """ # TODO: assign the lastest pose into self.robot_pose as a geometry_msgs.Pose object # first make sure that the particle weights are normalized self.normalize_particles() def update_particles_with_odom(self, msg): """ Implement a simple version of this (Level 1) or a more complex one (Level 2) """ new_odom_xy_theta = TransformHelpers.convert_pose_to_xy_and_theta(self.odom_pose.pose) # compute the change in x,y,theta since our last update if self.current_odom_xy_theta: old_odom_xy_theta = self.current_odom_xy_theta delta = (new_odom_xy_theta[0] - self.current_odom_xy_theta[0], new_odom_xy_theta[1] - self.current_odom_xy_theta[1], new_odom_xy_theta[2] - self.current_odom_xy_theta[2]) self.current_odom_xy_theta = new_odom_xy_theta else: self.current_odom_xy_theta = new_odom_xy_theta return # TODO: modify particles using delta # For added difficulty: Implement sample_motion_odometry (Prob Rob p 136) def map_calc_range(self,x,y,theta): """ Difficulty Level 3: implement a ray tracing likelihood model... Let me know if you are interested """ # TODO: nothing unless you want to try this alternate likelihood model pass def resample_particles(self): """ Resample the particles according to the new particle weights """ # make sure the distribution is normalized self.normalize_particles() # TODO: fill out the rest of the implementation def update_particles_with_laser(self, msg): """ Updates the particle weights in response to the scan contained in the msg """ # TODO: implement this pass @staticmethod def angle_normalize(z): """ convenience function to map an angle to the range [-pi,pi] """ return math.atan2(math.sin(z), math.cos(z)) @staticmethod def angle_diff(a, b): """ Calculates the difference between angle a and angle b (both should be in radians) the difference is always based on the closest rotation from angle a to angle b examples: angle_diff(.1,.2) -> -.1 angle_diff(.1, 2*math.pi - .1) -> .2 angle_diff(.1, .2+2*math.pi) -> -.1 """ a = ParticleFilter.angle_normalize(a) b = ParticleFilter.angle_normalize(b) d1 = a-b d2 = 2*math.pi - math.fabs(d1) if d1 > 0: d2 *= -1.0 if math.fabs(d1) < math.fabs(d2): return d1 else: return d2 @staticmethod def weighted_values(values, probabilities, size): """ Return a random sample of size elements form the set values with the specified probabilities values: the values to sample from (numpy.ndarray) probabilities: the probability of selecting each element in values (numpy.ndarray) size: the number of samples """ bins = np.add.accumulate(probabilities) return values[np.digitize(random_sample(size), bins)] def update_initial_pose(self, msg): """ Callback function to handle re-initializing the particle filter based on a pose estimate. These pose estimates could be generated by another ROS Node or could come from the rviz GUI """ xy_theta = TransformHelpers.convert_pose_to_xy_and_theta(msg.pose.pose) self.initialize_particle_cloud(xy_theta) self.fix_map_to_odom_transform(msg) def initialize_particle_cloud(self, xy_theta=None): """ Initialize the particle cloud. Arguments xy_theta: a triple consisting of the mean x, y, and theta (yaw) to initialize the particle cloud around. If this input is ommitted, the odometry will be used """ if xy_theta == None: xy_theta = TransformHelpers.convert_pose_to_xy_and_theta(self.odom_pose.pose) self.particle_cloud = [] # TODO create particles self.normalize_particles() self.update_robot_pose() def normalize_particles(self): """ Make sure the particle weights define a valid distribution (i.e. sum to 1.0) """ # TODO: implement this def publish_particles(self, msg): particles_conv = [] for p in self.particle_cloud: particles_conv.append(p.as_pose()) # actually send the message so that we can view it in rviz self.particle_pub.publish(PoseArray(header=Header(stamp=rospy.Time.now(),frame_id=self.map_frame),poses=particles_conv)) def scan_received(self, msg): """ This is the default logic for what to do when processing scan data. Feel free to modify this, however, I hope it will provide a good guide. The input msg is an object of type sensor_msgs/LaserScan """ if not(self.initialized): # wait for initialization to complete return if not(self.tf_listener.canTransform(self.base_frame,msg.header.frame_id,msg.header.stamp)): # need to know how to transform the laser to the base frame # this will be given by either Gazebo or neato_node return if not(self.tf_listener.canTransform(self.base_frame,self.odom_frame,msg.header.stamp)): # need to know how to transform between base and odometric frames # this will eventually be published by either Gazebo or neato_node return # calculate pose of laser relative ot the robot base p = PoseStamped(header=Header(stamp=rospy.Time(0),frame_id=msg.header.frame_id)) self.laser_pose = self.tf_listener.transformPose(self.base_frame,p) # find out where the robot thinks it is based on its odometry p = PoseStamped(header=Header(stamp=msg.header.stamp,frame_id=self.base_frame), pose=Pose()) self.odom_pose = self.tf_listener.transformPose(self.odom_frame, p) # store the the odometry pose in a more convenient format (x,y,theta) new_odom_xy_theta = TransformHelpers.convert_pose_to_xy_and_theta(self.odom_pose.pose) if not(self.particle_cloud): # now that we have all of the necessary transforms we can update the particle cloud self.initialize_particle_cloud() # cache the last odometric pose so we can only update our particle filter if we move more than self.d_thresh or self.a_thresh self.current_odom_xy_theta = new_odom_xy_theta # update our map to odom transform now that the particles are initialized self.fix_map_to_odom_transform(msg) elif (math.fabs(new_odom_xy_theta[0] - self.current_odom_xy_theta[0]) > self.d_thresh or math.fabs(new_odom_xy_theta[1] - self.current_odom_xy_theta[1]) > self.d_thresh or math.fabs(new_odom_xy_theta[2] - self.current_odom_xy_theta[2]) > self.a_thresh): # we have moved far enough to do an update! self.update_particles_with_odom(msg) # update based on odometry self.update_particles_with_laser(msg) # update based on laser scan self.resample_particles() # resample particles to focus on areas of high density self.update_robot_pose() # update robot's pose self.fix_map_to_odom_transform(msg) # update map to odom transform now that we have new particles # publish particles (so things like rviz can see them) self.publish_particles(msg) def fix_map_to_odom_transform(self, msg): """ Super tricky code to properly update map to odom transform... do not modify this... Difficulty level infinity. """ (translation, rotation) = TransformHelpers.convert_pose_inverse_transform(self.robot_pose) p = PoseStamped(pose=TransformHelpers.convert_translation_rotation_to_pose(translation,rotation),header=Header(stamp=msg.header.stamp,frame_id=self.base_frame)) self.odom_to_map = self.tf_listener.transformPose(self.odom_frame, p) (self.translation, self.rotation) = TransformHelpers.convert_pose_inverse_transform(self.odom_to_map.pose) def broadcast_last_transform(self): """ Make sure that we are always broadcasting the last map to odom transformation. This is necessary so things like move_base can work properly. """ if not(hasattr(self,'translation') and hasattr(self,'rotation')): return self.tf_broadcaster.sendTransform(self.translation, self.rotation, rospy.get_rostime(), self.odom_frame, self.map_frame)
class ParticleFilter: """ The class that represents a Particle Filter ROS Node Attributes list: initialized: a Boolean flag to communicate to other class methods that initializaiton is complete base_frame: the name of the robot base coordinate frame (should be "base_link" for most robots) map_frame: the name of the map coordinate frame (should be "map" in most cases) odom_frame: the name of the odometry coordinate frame (should be "odom" in most cases) scan_topic: the name of the scan topic to listen to (should be "scan" in most cases) n_particles: the number of particles in the filter d_thresh: the amount of linear movement before triggering a filter update a_thresh: the amount of angular movement before triggering a filter update laser_max_distance: the maximum distance to an obstacle we should use in a likelihood calculation pose_listener: a subscriber that listens for new approximate pose estimates (i.e. generated through the rviz GUI) particle_pub: a publisher for the particle cloud laser_subscriber: listens for new scan data on topic self.scan_topic tf_listener: listener for coordinate transforms tf_broadcaster: broadcaster for coordinate transforms particle_cloud: a list of particles representing a probability distribution over robot poses current_odom_xy_theta: the pose of the robot in the odometry frame when the last filter update was performed. The pose is expressed as a list [x,y,theta] (where theta is the yaw) map: the map we will be localizing ourselves in. The map should be of type nav_msgs/OccupancyGrid """ xy_theta = [] def __init__(self): self.initialized = False # make sure we don't perform updates before everything is setup rospy.init_node('pf') # tell roscore that we are creating a new node named "pf" self.base_frame = "base_link" # the frame of the robot base self.map_frame = "map" # the name of the map coordinate frame self.odom_frame = "odom" # the name of the odometry coordinate frame self.scan_topic = "scan" # the topic where we will get laser scans from self.n_particles = 250 # the number of particles to use self.d_thresh = 0.2 # the amount of linear movement before performing an update self.a_thresh = math.pi/6 # the amount of angular movement before performing an update self.laser_max_distance = 2.0 # maximum penalty to assess in the likelihood field model self.model_noise_rate = 0.15 # TODO: define additional constants if needed # Setup pubs and subs # pose_listener responds to selection of a new approximate robot location (for instance using rviz) self.pose_listener = rospy.Subscriber("initialpose", PoseWithCovarianceStamped, self.update_initial_pose) print() # publish the current particle cloud. This enables viewing particles in rviz. self.particle_pub = rospy.Publisher("particlecloud", PoseArray, queue_size=10) # laser_subscriber listens for data from the lidar self.laser_subscriber = rospy.Subscriber(self.scan_topic, LaserScan, self.scan_received) # enable listening for and broadcasting coordinate transforms self.tf_listener = TransformListener() self.tf_broadcaster = TransformBroadcaster() self.particle_cloud = [] self.current_odom_xy_theta = [] # request the map from the map server, the map should be of type nav_msgs/OccupancyGrid # TODO: fill in the appropriate service call here. The resultant map should be assigned be passed # into the init method for OccupancyField rospy.wait_for_service('static_map') grid = rospy.ServiceProxy('static_map',GetMap) my_map = grid().map # for now we have commented out the occupancy field initialization until you can successfully fetch the map self.field = OccupancyField(my_map) self.initialized = True def create_initial_particle_list(self,xy_theta): init_particle_list = [] n = self.n_particles for i in range(self.n_particles): w = 1.0/n x = gauss(xy_theta[0],0.5) y = gauss(xy_theta[1],0.5) theta = gauss(xy_theta[2],((math.pi)/2)) particle = Particle(x,y,theta,w) init_particle_list.append(particle) print("init_particle_list") return init_particle_list def update_robot_pose(self): """ Update the estimate of the robot's pose given the updated particles. There are two logical methods for this: (1): compute the mean pose (2): compute the most likely pose (i.e. the mode of the distribution) """ # first make sure that the particle weights are normalized self.normalize_particles() # TODO: assign the lastest pose into self.robot_pose as a geometry_msgs.Pose object # just to get started we will fix the robot's pose to always be at the origin x = 0 y = 0 theta = 0 angles = [] for particle in self.particle_cloud: x += particle.x * particle.w y += particle.y * particle.w v = [particle.w * math.cos(math.radians(particle.theta)), particle.w * math.sin(math.radians(particle.theta))] angles.append(v) theta = sum_vectors(angles) orientation = tf.transformations.quaternion_from_euler(0,0,theta) self.robot_pose = Pose(position=Point(x=x,y=y),orientation=Quaternion(x=orientation[0], y=orientation[1], z=orientation[2], w=orientation[3])) def update_particles_with_odom(self, msg): """ Update the particles using the newly given odometry pose. The function computes the value delta which is a tuple (x,y,theta) that indicates the change in position and angle between the odometry when the particles were last updated and the current odometry. msg: this is not really needed to implement this, but is here just in case. """ print('update_w_odom') new_odom_xy_theta = convert_pose_to_xy_and_theta(self.odom_pose.pose) # compute the change in x,y,theta since our last update if self.current_odom_xy_theta: old_odom_xy_theta = self.current_odom_xy_theta delta = (new_odom_xy_theta[0] - self.current_odom_xy_theta[0], new_odom_xy_theta[1] - self.current_odom_xy_theta[1], new_odom_xy_theta[2] - self.current_odom_xy_theta[2]) self.current_odom_xy_theta = new_odom_xy_theta for particle in self.particle_cloud: parc = (math.atan2(delta[1], delta[0]) - old_odom_xy_theta[2]) % 360 particle.x += (math.sqrt((delta[0]**2) + (delta[1]**2)))* math.cos(parc) particle.y += (math.sqrt((delta[0]**2) + (delta[1]**2))) * math.sin(parc) particle.theta += delta[2] else: self.current_odom_xy_theta = new_odom_xy_theta return #DONE # TODO: modify particles using delta # For added difficulty: Implement sample_motion_odometry (Prob Rob p 136) def map_calc_range(self,x,y,theta): """ Difficulty Level 3: implement a ray tracing likelihood model... Let me know if you are interested """ # TODO: nothing unless you want to try this alternate likelihood model pass def resample_particles(self): """ Resample the particles according to the new particle weights. The weights stored with each particle should define the probability that a particular particle is selected in the resampling step. You may want to make use of the given helper function draw_random_sample. """ # make sure the distribution is normalized self.normalize_particles() values = np.empty(self.n_particles) probs = np.empty(self.n_particles) for i in range(len(self.particle_cloud)): values[i] = i probs[i] = self.particle_cloud[i].w new_random_particle = ParticleFilter.weighted_values(values,probs,self.n_particles) new_particles = [] for i in new_random_particle: idx = int(i) s_p = self.particle_cloud[idx] new_particles.append(Particle(x=s_p.x+gauss(0,.025),y=s_p.y+gauss(0,.05),theta=s_p.theta+gauss(0,.05))) self.particle_cloud = new_particles self.normalize_particles() def update_particles_with_laser(self, msg): """ Updates the particle weights in response to the scan contained in the msg """ print('update_w_laser') readings = msg.ranges for particle in self.particle_cloud: for read in range(0,len(readings),3): self.field.get_particle_likelyhood(particle,readings[read],self.model_noise_rate,read) self.normalize_particles() self.resample_particles() @staticmethod def weighted_values(values, probabilities, size): """ Return a random sample of size elements from the set values with the specified probabilities values: the values to sample from (numpy.ndarray) probabilities: the probability of selecting each element in values (numpy.ndarray) size: the number of samples """ bins = np.add.accumulate(probabilities) return values[np.digitize(random_sample(size), bins)-1] @staticmethod def draw_random_sample(choices, probabilities, n): """ Return a random sample of n elements from the set choices with the specified probabilities choices: the values to sample from represented as a list probabilities: the probability of selecting each element in choices represented as a list n: the number of samples """ print('draw_random_sample') values = np.array(range(len(choices))) probs = np.array(probabilities) bins = np.add.accumulate(probs) inds = values[np.digitize(random_sample(n), bins)] samples = [] for i in inds: samples.append(deepcopy(choices[int(i)])) return samples def update_initial_pose(self, msg): """ Callback function to handle re-initializing the particle filter based on a pose estimate. These pose estimates could be generated by another ROS Node or could come from the rviz GUI """ xy_theta = convert_pose_to_xy_and_theta(msg.pose.pose) self.initialize_particle_cloud(xy_theta) self.fix_map_to_odom_transform(msg) def initialize_particle_cloud(self, xy_theta=None): """ Initialize the particle cloud. Arguments xy_theta: a triple consisting of the mean x, y, and theta (yaw) to initialize the particle cloud around. If this input is ommitted, the odometry will be used """ if xy_theta == None: xy_theta = convert_pose_to_xy_and_theta(self.odom_pose.pose) #self.particle_cloud = [] # TODO create particles self.particle_cloud = self.create_initial_particle_list(xy_theta) self.normalize_particles() self.update_robot_pose() def normalize_particles(self): """ Make sure the particle weights define a valid distribution (i.e. sum to 1.0) """ w_sum = sum([p.w for p in self.particle_cloud]) for particle in self.particle_cloud: particle.w /= w_sum # TODO: implement this def publish_particles(self, msg): print('publish_particles') particles_conv = [] for p in self.particle_cloud: particles_conv.append(p.as_pose()) # actually send the message so that we can view it in rviz self.particle_pub.publish(PoseArray(header=Header(stamp=rospy.Time.now(), frame_id=self.map_frame), poses=particles_conv)) def scan_received(self, msg): """ This is the default logic for what to do when processing scan data. Feel free to modify this, however, I hope it will provide a good guide. The input msg is an object of type sensor_msgs/LaserScan """ print('scan_received') if not(self.initialized): # wait for initialization to complete return if not(self.tf_listener.canTransform(self.base_frame,msg.header.frame_id,msg.header.stamp)): # need to know how to transform the laser to the base frame # this will be given by either Gazebo or neato_node return if not(self.tf_listener.canTransform(self.base_frame,self.odom_frame,msg.header.stamp)): # need to know how to transform between base and odometric frames # this will eventually be published by either Gazebo or neato_node return # calculate pose of laser relative ot the robot base p = PoseStamped(header=Header(stamp=rospy.Time(0), frame_id=msg.header.frame_id)) self.laser_pose = self.tf_listener.transformPose(self.base_frame,p) # find out where the robot thinks it is based on its odometry p = PoseStamped(header=Header(stamp=rospy.Time(0), frame_id=self.base_frame), pose=Pose()) self.odom_pose = self.tf_listener.transformPose(self.odom_frame, p) # store the the odometry pose in a more convenient format (x,y,theta) new_odom_xy_theta = convert_pose_to_xy_and_theta(self.odom_pose.pose) if not(self.particle_cloud): # now that we have all of the necessary transforms we can update the particle cloud self.initialize_particle_cloud() # cache the last odometric pose so we can only update our particle filter if we move more than self.d_thresh or self.a_thresh self.current_odom_xy_theta = new_odom_xy_theta # update our map to odom transform now that the particles are initialized self.fix_map_to_odom_transform(msg) elif (math.fabs(new_odom_xy_theta[0] - self.current_odom_xy_theta[0]) > self.d_thresh or math.fabs(new_odom_xy_theta[1] - self.current_odom_xy_theta[1]) > self.d_thresh or math.fabs(new_odom_xy_theta[2] - self.current_odom_xy_theta[2]) > self.a_thresh): # we have moved far enough to do an update! self.update_particles_with_odom(msg) # update based on odometry self.update_particles_with_laser(msg) # update based on laser scan self.update_robot_pose() # update robot's pose self.resample_particles() # resample particles to focus on areas of high density self.fix_map_to_odom_transform(msg) # update map to odom transform now that we have new particles # publish particles (so things like rviz can see them) self.publish_particles(msg) def fix_map_to_odom_transform(self, msg): """ This method constantly updates the offset of the map and odometry coordinate systems based on the latest results from the localizer """ (translation, rotation) = convert_pose_inverse_transform(self.robot_pose) p = PoseStamped(pose=convert_translation_rotation_to_pose(translation,rotation), header=Header(stamp=rospy.Time(0),frame_id=self.base_frame)) self.odom_to_map = self.tf_listener.transformPose(self.odom_frame, p) (self.translation, self.rotation) = convert_pose_inverse_transform(self.odom_to_map.pose) def broadcast_last_transform(self): """ Make sure that we are always broadcasting the last map to odom transformation. This is necessary so things like move_base can work properly. """ if not(hasattr(self,'translation') and hasattr(self,'rotation')): return print('broadcast') self.tf_broadcaster.sendTransform(self.translation, self.rotation, rospy.Time.now(), self.odom_frame, self.map_frame)
class ArtArmNavigationActionServer(object): feedback = ArmNavigationFeedback() result = ArmNavigationResult() def __init__(self, server_prefix, group_name): self.group_name = group_name self.group = moveit_commander.MoveGroupCommander(self.group_name) self._as = actionlib.SimpleActionServer( server_prefix + self.group_name + "/manipulation", ArmNavigationAction, self.action_cb) self._as.start() char = self.group_name[0] self.jta = actionlib.SimpleActionClient( '/' + char + '_arm_controller/joint_trajectory_action', JointTrajectoryAction) rospy.loginfo('Waiting for joint trajectory action') self.jta.wait_for_server() rospy.loginfo('Got it') self.joint_state_sub = rospy.Subscriber("/joint_states", JointState, self.js_cb, queue_size=1) self.angles = None self.move_to_pose_pub = rospy.Publisher( "/pr2_arm_navigation/move_to_pose", PoseStamped, queue_size=10, latch=True) self.joint_names = [ char + '_shoulder_pan_joint', char + '_shoulder_lift_joint', char + '_upper_arm_roll_joint', char + '_elbow_flex_joint', char + '_forearm_roll_joint', char + '_wrist_flex_joint', char + '_wrist_roll_joint' ] self.tf_listener = TransformListener() self.look_at_pub = rospy.Publisher("/art/robot/look_at", PointStamped, queue_size=1) def action_cb(self, goal): max_attempt = 3 self.feedback.attempt = 1 rospy.loginfo("Got goal with " + str(len(goal.poses)) + "poses.") if goal.operation == ArmNavigationGoal.MOVE_THROUGH_POSES: for idx, p in enumerate(goal.poses): attempt = 1 self._as.publish_feedback(self.feedback) while not self.move_to_point(p): attempt += 1 if attempt > max_attempt: self.result.result = self.result.FAILURE self._as.set_aborted(self.result) return self.result.result = ArmNavigationResult.SUCCESS elif goal.operation == ArmNavigationGoal.TOUCH_POSES: for idx, p in enumerate(goal.poses): attempt = 1 self._as.publish_feedback(self.feedback) # TODO it would be better to only repeat failed step (e.g. go3) world_frame = rospy.get_param("/art/conf/world_frame", "marker") p.header.stamp = rospy.Time(0) p_marker = self.tf_listener.transformPose(world_frame, p) """while not self.touch_point(p_marker, goal.drill_duration): rospy.loginfo("Attempt " + str(attempt) + " out of " + str(max_attempt) + ".") attempt += 1 if attempt > max_attempt: self.result.result = self.result.FAILURE self._as.set_aborted(self.result) return""" if self.touch_point(p_marker, goal.drill_duration): self.result.result = self.result.SUCCESS self._as.set_succeeded(result=self.result) else: self.result.result = self.result.FAILURE self.result.message = "Failed to drill" self._as.set_aborted(result=self.result) elif goal.operation == ArmNavigationGoal.MOVE_THROUGH_TRAJECTORY: self.result.result = ArmNavigationResult.BAD_REQUEST self.result.message = "Not implemented!" self._as.set_aborted(self.result) return rospy.loginfo("return") self.result.result = self.result.SUCCESS self._as.set_succeeded(self.result) def move_to_point(self, pose): pt = PointStamped() pt.header = pose.header pt.point = pose.pose.position self.look_at_pub.publish(pt) self.group.set_pose_target(pose) self.move_to_pose_pub.publish(pose) if not self.group.go(wait=True): return False return True def touch_point(self, pose, drill_duration, max_attempts=3): rospy.loginfo("Touch point in") pre_touch_pose = copy.deepcopy(pose) # TODO this works only for world frame_id (marker) -> in general, this pre_touch translation should be done with respect to x-axis of the gripper! pre_touch_pose.pose.position.z += 0.05 # 5cm above desired pose self.group.set_pose_target(pre_touch_pose) rospy.logdebug("Pre touch pose") self.move_to_pose_pub.publish(pre_touch_pose) attempt = 0 while not self.group.go(wait=True): if attempt > max_attempts: rospy.logerr("Failed to reach pre touch pose, giving up") return False else: attempt += 1 rospy.logerr("Pre touch pose failed, try again") self.group.set_pose_target(pose) rospy.logdebug("Touching point") self.move_to_pose_pub.publish(pose) attempt = 0 while not self.group.go(wait=True): if attempt > max_attempts: rospy.logerr("Failed to reach touch pose, giving up") return False else: attempt += 1 rospy.logerr("Touch pose failed, try again") self.rotate_gripper(drill_duration) self.group.set_pose_target(pre_touch_pose) rospy.loginfo("Post touch pose") self.move_to_pose_pub.publish(pre_touch_pose) attempt = 0 while not self.group.go(wait=True): if attempt > max_attempts: rospy.logerr("Failed to reach post touch pose, giving up") return False else: attempt += 1 rospy.logerr("Post touch pose failed, try again") rospy.logdebug("Touch done") return True def rotate_gripper(self, duration): rospy.loginfo("rotate in, duration = " + str(duration)) if duration == 0: return goal = JointTrajectoryGoal() goal.trajectory.joint_names = self.joint_names point = JointTrajectoryPoint() angles = self.angles rate = 4 for i in xrange(duration * rate): angles[6] += i * 0.1 rospy.loginfo(angles[6]) point.positions = angles point.time_from_start = rospy.Duration(1 / rate * i) goal.trajectory.points.append(copy.deepcopy(point)) rospy.loginfo("rotate send goal") self.jta.send_goal(goal) rospy.sleep(duration + 1) rospy.loginfo("rotate out") def js_cb(self, data): self.angles = [0] * 7 positions = zip(data.name, data.position) for name, position in positions: if name in self.joint_names: self.angles[self.joint_names.index(name)] = position
class ArmActions(): def __init__(self): rospy.init_node('left_arm_actions') self.tfl = TransformListener() self.aul = l_arm_utils.ArmUtils(self.tfl) #self.fth = ft_handler.FTHandler() rospy.loginfo("Waiting for l_utility_frame_service") try: rospy.wait_for_service('/l_utility_frame_update', 7.0) self.aul.update_frame = rospy.ServiceProxy( '/l_utility_frame_update', FrameUpdate) rospy.loginfo("Found l_utility_frame_service") except: rospy.logwarn("Left Utility Frame Service Not available") rospy.loginfo('Waiting for Pixel_2_3d Service') try: rospy.wait_for_service('/pixel_2_3d', 7.0) self.p23d_proxy = rospy.ServiceProxy('/pixel_2_3d', Pixel23d, True) rospy.loginfo("Found pixel_2_3d service") except: rospy.logwarn("Pixel_2_3d Service Not available") #Low-level motion requests: pass directly to arm_utils rospy.Subscriber('wt_left_arm_pose_commands', Point, self.torso_frame_move) rospy.Subscriber('wt_left_arm_angle_commands', JointTrajectoryPoint, self.aul.send_traj_point) #More complex motion scripts, defined here using arm_util functions rospy.Subscriber('norm_approach_left', PoseStamped, self.norm_approach) rospy.Subscriber('wt_grasp_left_goal', PoseStamped, self.grasp) #rospy.Subscriber('wt_wipe_left_goals', PoseStamped, self.wipe) rospy.Subscriber('wt_wipe_left_goals', PoseStamped, self.force_wipe_agg) rospy.Subscriber('wt_swipe_left_goals', PoseStamped, self.swipe) rospy.Subscriber('wt_lin_move_left', Float32, self.hand_move) rospy.Subscriber('wt_adjust_elbow_left', Float32, self.aul.adjust_elbow) rospy.Subscriber('wt_surf_wipe_left_points', Point, self.prep_surf_wipe) rospy.Subscriber('wt_poke_left_point', PoseStamped, self.poke) rospy.Subscriber('wt_contact_approach_left', PoseStamped, self.approach_to_contact) self.wt_log_out = rospy.Publisher('wt_log_out', String) self.test_pose = rospy.Publisher('test_pose', PoseStamped, latch=True) self.say = rospy.Publisher('text_to_say', String) self.wipe_started = False self.surf_wipe_started = False self.wipe_ends = [PoseStamped(), PoseStamped()] #FORCE_TORQUE ADDITIONS rospy.Subscriber('netft_gravity_zeroing/wrench_zeroed', WrenchStamped, self.ft_preprocess) self.rezero_wrench = rospy.Publisher( 'netft_gravity_zeroing/rezero_wrench', Bool) #rospy.Subscriber('ft_data_pm_adjusted', WrenchStamped, self.ft_preprocess) rospy.Subscriber('wt_ft_goal', Float32, self.set_force_goal) self.wt_force_out = rospy.Publisher('wt_force_out', Float32) self.ft_rate_limit = rospy.Rate(30) self.ft = WrenchStamped() self.ft_mag = 0. self.ft_case = None self.force_goal_mean = 1.42 self.force_goal_std = 0.625 self.stop_maintain = False self.force_wipe_started = False self.force_wipe_start = PoseStamped() self.force_wipe_appr = PoseStamped() def set_force_goal(self, msg): self.force_goal_mean = msg.data def ft_preprocess(self, ft): self.ft = ft self.ft_mag = math.sqrt(ft.wrench.force.x**2 + ft.wrench.force.y**2 + ft.wrench.force.z**2) #print 'Force Magnitude: ', self.ft_mag self.wt_force_out.publish(self.ft_mag) def approach_to_contact(self, ps, overshoot=0.05): self.stop_maintain = True self.aul.update_frame(ps) appr = self.aul.find_approach(ps, 0.15) goal = self.aul.find_approach(ps, -overshoot) (appr_reachable, ik_goal) = self.aul.full_ik_check(appr) (goal_reachable, ik_goal) = self.aul.full_ik_check(goal) if appr_reachable and goal_reachable: traj = self.aul.build_trajectory(goal, appr, tot_points=600) prep = self.aul.move_arm_to(appr) if prep: curr_traj_point = self.advance_to_contact(traj) self.maintain_norm_force(traj, curr_traj_point) else: rospy.loginfo("Cannot reach desired 'move-to-contact' point") self.wt_log_out.publish( data="Cannot reach desired 'move-to-contact' point") def advance_to_contact(self, traj): completed = False curr_traj_point = 0 advance_rate = rospy.Rate(30) while not (rospy.is_shutdown() or completed): if not (curr_traj_point >= len(traj.points)): self.aul.send_traj_point(traj.points[curr_traj_point], 0.05) #hfm_pose = self.aul.hand_frame_move(0.003) #self.aul.blind_move(hfm_pose,0.9) advance_rate.sleep() curr_traj_point += 1 else: rospy.loginfo( "Moved past expected contact, but no contact found!") self.wt_log_out.publish( data="Moved past expected contact, but no contact found!") completed = True if self.ft.wrench.force.z < -1.5: completed = True return curr_traj_point def maintain_norm_force(self, traj, curr_traj_point=0, mean=0, std=1): self.stop_maintain = False maintain_rate = rospy.Rate(100) while not (rospy.is_shutdown() or self.stop_maintain): if self.ft.wrench.force.z > mean + std: curr_traj_point += 1 if not (curr_traj_point >= len(traj.points)): self.aul.send_traj_point(traj.points[curr_traj_point], 0.033) rospy.sleep(0.033) else: rospy.loginfo( "Force too low, but extent of the trajectory is reached" ) self.wt_log_out.publish( data= "Force too low, but extent of the trajectory is reached" ) stopped = True elif self.ft.wrench.force.z < mean - std: curr_traj_point -= 1 if curr_traj_point >= 0: self.aul.send_traj_point(traj.points[curr_traj_point], 0.033) rospy.sleep(0.033) else: rospy.loginfo( "Beginning of trajectory reached, cannot back away further" ) self.wt_log_out.publish( data= "Beginning of trajectory reached, cannot back away further" ) stopped = True maintain_rate.sleep() mean = -self.force_goal_mean std = self.force_goal_std # def maintain_net_force(self, mean=0, std=3): # self.stop_maintain = False # maintain_rate = rospy.Rate(100) # while not (rospy.is_shutdown() or self.stop_maintain): # if self.ft_mag > mean + 8: # curr_angs = self.aul.joint_state_act.positions # try: # x_plus = np.subtract(self.aul.ik_pose_proxy(self.aul.form_ik_request(self.aul.hand_frame_move(0.02))).solution.joint_state.position, curr_angs) # x_minus = np.subtract(self.aul.ik_pose_proxy(self.aul.form_ik_request(self.aul.hand_frame_move(-0.02))).solution.joint_state.position, curr_angs) # y_plus = np.subtract(self.aul.ik_pose_proxy(self.aul.form_ik_request(self.aul.hand_frame_move(0, 0.02, 0))).solution.joint_state.position, curr_angs) # y_minus = np.subtract(self.aul.ik_pose_proxy(self.aul.form_ik_request(self.aul.hand_frame_move(0, -0.02, 0))).solution.joint_state.position, curr_angs) # z_plus = np.subtract(self.aul.ik_pose_proxy(self.aul.form_ik_request(self.aul.hand_frame_move(0, 0, 0.02))).solution.joint_state.position, curr_angs) # z_minus = np.subtract(self.aul.ik_pose_proxy(self.aul.form_ik_request(self.aul.hand_frame_move(0, 0, -0.02))).solution.joint_state.position, curr_angs) # #print 'x: ', x_plus,'\r\n', x_minus # #print 'y: ', y_plus,'\r\n', y_minus # #print 'z: ', z_plus,'\r\n', z_minus # ft_sum = self.ft_mag # parts = np.divide([self.ft.wrench.force.x, self.ft.wrench.force.y, self.ft.wrench.force.z], ft_sum) # print 'parts', parts # ends = [[x_plus,x_minus],[y_plus, y_minus],[z_plus,z_minus]] # side = [[0]*7]*3 # for i, part in enumerate(parts): # if part >=0: # side[i] = ends[i][0] # else: # side[i] = ends[i][1] # # ang_out = curr_angs # for i in range(3): # ang_out -= np.average(side, 0, parts) # except: # print 'Near Joint Limits!' # self.aul.send_joint_angles(ang_out) # # #print 'x: ', x_plus, x_minus # maintain_rate.sleep() # mean = self.force_goal_mean # std = self.force_goal_std def maintain_net_force(self, mean=0, std=8): self.stop_maintain = False maintain_rate = rospy.Rate(100) while not (rospy.is_shutdown() or self.stop_maintain): if self.ft_mag > mean + 3: #print 'Moving to avoid force' goal = PoseStamped() goal.header.frame_id = 'l_netft_frame' goal.header.stamp = rospy.Time(0) goal.pose.position.x = -np.clip(self.ft.wrench.force.x / 2500, -0.1, 0.1) goal.pose.position.y = -np.clip(self.ft.wrench.force.y / 2500, -0.1, 0.1) goal.pose.position.z = -np.clip(self.ft.wrench.force.z / 2500, -0.1, 0.1) + 0.052 goal = self.tfl.transformPose('/torso_lift_link', goal) goal.header.stamp = rospy.Time.now() goal.pose.orientation = self.aul.curr_pose().pose.orientation self.test_pose.publish(goal) self.aul.fast_move(goal, 0.2) rospy.sleep(0.05) maintain_rate.sleep() mean = self.force_goal_mean std = self.force_goal_std #def mannequin(self): #mannequin_rate=rospy.Rate(10) #while not rospy.is_shutdown(): #joints = np.add(np.array(self.aul.joint_state_act.positions), # np.clip(np.array(self.aul.joint_state_err.positions), -0.05, 0.05)) #joints = self.aul.joint_state_act.positions #print joints #raw_input('Review Joint Angles') #self.aul.send_joint_angles(joints,0.1) #mannequin_rate.sleep() def force_wipe_agg(self, ps): self.aul.update_frame(ps) rospy.sleep(0.1) pose = self.aul.find_approach(ps, 0) (goal_reachable, ik_goal) = self.aul.ik_check(pose) if goal_reachable: if not self.force_wipe_started: appr = self.aul.find_approach(ps, 0.20) (appr_reachable, ik_goal) = self.aul.ik_check(appr) self.test_pose.publish(appr) if appr_reachable: self.force_wipe_start = pose self.force_wipe_appr = appr self.force_wipe_started = True else: rospy.loginfo( "Cannot reach approach point, please choose another") self.wt_log_out.publish( data= "Cannot reach approach point, please choose another") self.say.publish( data= "I cannot get to a safe approach for there, please choose another point" ) else: self.force_wipe(self.force_wipe_start, pose) self.force_wipe_started = False else: rospy.loginfo("Cannot reach wiping point, please choose another") self.wt_log_out.publish( data="Cannot reach wiping point, please choose another") self.say.publish( data="I cannot reach there, please choose another point") def force_wipe(self, ps_start, ps_finish, travel=0.05): ps_start.header.stamp = rospy.Time.now() ps_finish.header.stamp = rospy.Time.now() ps_start_far = self.aul.hand_frame_move(travel, 0, 0, ps_start) ps_start_near = self.aul.hand_frame_move(-travel, 0, 0, ps_start) ps_finish_far = self.aul.hand_frame_move(travel, 0, 0, ps_finish) ps_finish_near = self.aul.hand_frame_move(-travel, 0, 0, ps_finish) n_points = int( math.ceil(self.aul.calc_dist(ps_start, ps_finish) * 7000)) print 'n_points: ', n_points mid_traj = self.aul.build_trajectory(ps_finish, ps_start, tot_points=n_points) near_traj = self.aul.build_trajectory(ps_finish_near, ps_start_near, tot_points=n_points) far_traj = self.aul.build_trajectory(ps_finish_far, ps_start_far, tot_points=n_points) near_angles = [ list(near_traj.points[i].positions) for i in range(n_points) ] mid_angles = [ list(mid_traj.points[i].positions) for i in range(n_points) ] far_angles = [ list(far_traj.points[i].positions) for i in range(n_points) ] fmn_diff = np.abs(np.subtract(near_angles, far_angles)) fmn_max = np.max(fmn_diff, axis=0) print 'fmn_max: ', fmn_max if any(fmn_max > math.pi / 2): rospy.loginfo( "TOO LARGE OF AN ANGLE CHANGE ALONG GRADIENT, IK REGION PROBLEMS!" ) self.wt_log_out.publish( data= "The path requested required large movements (IK Limits cause angle wrapping)" ) self.say.publish( data="Large motion detected, cancelling. Please try again.") return for i in range(7): n_max = max(np.abs(np.diff(near_angles, axis=0)[i])) m_max = max(np.abs(np.diff(mid_angles, axis=0)[i])) f_max = max(np.abs(np.diff(far_angles, axis=0)[i])) n_mean = 4 * np.mean(np.abs(np.diff(near_angles, axis=0)[i])) m_mean = 4 * np.mean(np.abs(np.diff(mid_angles, axis=0)[i])) f_mean = 4 * np.mean(np.abs(np.diff(far_angles, axis=0)[i])) print 'near: max: ', n_max, 'mean: ', n_mean print 'mid: max: ', m_max, 'mean: ', m_mean print 'far: max: ', f_max, 'mean: ', f_mean if (n_max >= n_mean) or (m_max >= m_mean) or (f_max >= f_mean): rospy.logerr( "TOO LARGE OF AN ANGLE CHANGE ALONG PATH, IK REGION PROBLEMS!" ) self.wt_log_out.publish( data= "The path requested required large movements (IK Limits cause angle wrapping)" ) self.say.publish( data="Large motion detected, cancelling. Please try again." ) return near_diff = np.subtract(near_angles, mid_angles).tolist() far_diff = np.subtract(far_angles, mid_angles).tolist() self.say.publish(data="Moving to approach point") prep = self.aul.move_arm_to(self.force_wipe_appr) if prep: print 'cur angles: ', self.aul.joint_state_act.positions, 'near angs: ', near_angles[ 0] print np.abs( np.subtract(self.aul.joint_state_act.positions, near_angles[0])) if np.max( np.abs( np.subtract(self.aul.joint_state_act.positions, near_angles[0]))) > math.pi: self.say.publish(data="Adjusting for-arm roll") print "Evasive Action!" angles = list(self.aul.joint_state_act.positions) flex = angles[5] angles[5] = -0.1 self.aul.send_joint_angles(angles, 4) rospy.sleep(4) angles[4] = near_angles[0][4] self.aul.send_joint_angles(angles, 6) rospy.sleep(6) angles[5] = flex self.aul.send_joint_angles(angles, 4) rospy.sleep(4) self.say.publish(data="Making Approach.") self.aul.send_joint_angles(np.add(mid_angles[0], near_diff[0]), 7.5) rospy.sleep(7) self.rezero_wrench.publish(data=True) rospy.sleep(1) wipe_rate = rospy.Rate(400) self.stop_maintain = False count = 0 lap = 0 max_laps = 4 mean = self.force_goal_mean std = self.force_goal_std bias = 1 self.say.publish(data="Wiping") while not (rospy.is_shutdown() or self.stop_maintain) and ( count + 1 <= n_points) and (lap < max_laps): #print "mean: ", mean, "std: ", std, "force: ", self.ft_mag if self.ft_mag >= mean + std: # print 'Force too high!' bias += (self.ft_mag / 3500) elif self.ft_mag < mean - std: # print 'Force too low' bias -= max(0.001, (self.ft_mag / 3500)) else: # print 'Force Within Range' count += 1 bias = np.clip(bias, -1, 1) if bias > 0.: diff = near_diff[count] else: diff = far_diff[count] angles_out = np.add(mid_angles[count], np.multiply(abs(bias), diff)) self.aul.send_joint_angles(angles_out, 0.008) rospy.sleep(0.0075) mean = self.force_goal_mean std = self.force_goal_std if count + 1 >= n_points: count = 0 mid_angles.reverse() near_diff.reverse() far_diff.reverse() lap += 1 #if lap == 3: # self.say.publish(data="Hold Still, you rascal!") rospy.sleep(0.5) self.say.publish(data="Pulling away") self.aul.send_joint_angles(near_angles[0], 5) rospy.sleep(5) self.say.publish(data="Finished wiping. Thank you") def torso_frame_move(self, msg): self.stop_maintain = True goal = self.aul.curr_pose() goal.pose.position.x += msg.x goal.pose.position.y += msg.y goal.pose.position.z += msg.z self.aul.blind_move(goal) def hand_move(self, f32): self.stop_maintain = True hfm_pose = self.aul.hand_frame_move(f32.data) self.aul.blind_move(hfm_pose) def norm_approach(self, pose): self.stop_maintain = True self.aul.update_frame(pose) appr = self.aul.find_approach(pose, 0.15) self.aul.move_arm_to(appr) def grasp(self, ps): self.stop_maintain = True rospy.loginfo("Initiating Grasp Sequence") self.wt_log_out.publish(data="Initiating Grasp Sequence") self.aul.update_frame(ps) approach = self.aul.find_approach(ps, 0.15) rospy.loginfo("approach: \r\n %s" % approach) at_appr = self.aul.move_arm_to(approach) rospy.loginfo("arrived at approach: %s" % at_appr) if at_appr: opened = self.aul.gripper(0.09) if opened: rospy.loginfo("making linear approach") #hfm_pose = self.aul.hand_frame_move(0.23) hfm_pose = self.aul.find_approach(ps, -0.02) self.aul.blind_move(hfm_pose) self.aul.wait_for_stop(2) closed = self.aul.gripper(0.0) if not closed: rospy.loginfo( "Couldn't close completely: Grasp likely successful") hfm_pose = self.aul.hand_frame_move(-0.23) self.aul.blind_move(hfm_pose) else: pass def prep_surf_wipe(self, point): pixel_u = point.x pixel_v = point.y test_pose = self.p23d_proxy(pixel_u, pixel_v).pixel3d self.aul.update_frame(test_pose) test_pose = self.aul.find_approach(test_pose, 0) (reachable, ik_goal) = self.aul.full_ik_check(test_pose) if reachable: if not self.surf_wipe_started: start_pose = test_pose self.surf_wipe_start = [pixel_u, pixel_v, start_pose] self.surf_wipe_started = True rospy.loginfo( "Received valid starting position for wiping action") self.wt_log_out.publish( data="Received valid starting position for wiping action") return #Return after 1st point, wait for second else: rospy.loginfo( "Received valid ending position for wiping action") self.wt_log_out.publish( data="Received valid ending position for wiping action") self.surf_wipe_started = False #Continue on successful 2nd point else: rospy.loginfo("Cannot reach wipe position, please try another") self.wt_log_out.publish( data="Cannot reach wipe position, please try another") return #Return on invalid point, wait for another dist = self.aul.calc_dist(self.surf_wipe_start[2], test_pose) print 'dist', dist num_points = dist / 0.02 print 'num_points', num_points us = np.round(np.linspace(self.surf_wipe_start[0], pixel_u, num_points)) vs = np.round(np.linspace(self.surf_wipe_start[1], pixel_v, num_points)) surf_points = [PoseStamped() for i in xrange(len(us))] print "Surface Points", [us, vs] for i in xrange(len(us)): pose = self.p23d_proxy(us[i], vs[i]).pixel3d self.aul.update_frame(pose) surf_points[i] = self.aul.find_approach(pose, 0) print i + 1, '/', len(us) self.aul.blind_move(surf_points[0]) self.aul.wait_for_stop() for pose in surf_points: self.aul.blind_move(pose, 2.5) rospy.sleep(2) #self.aul.wait_for_stop() self.aul.hand_frame_move(-0.1) def poke(self, ps): self.stop_maintain = True self.aul.update_frame(ps) appr = self.aul.find_approach(ps, 0.15) touch = self.aul.find_approach(ps, 0) prepared = self.aul.move_arm_to(appr) if prepared: self.aul.blind_move(touch) self.aul.wait_for_stop() rospy.sleep(7) self.aul.blind_move(appr) def swipe(self, ps): traj = self.prep_wipe(ps) if traj is not None: self.stop_maintain = True self.wipe_move(traj, 1) def wipe(self, ps): traj = self.prep_wipe(ps) if traj is not None: self.stop_maintain = True self.wipe_move(traj, 4) def prep_wipe(self, ps): #print "Prep Wipe Received: %s" %pa self.aul.update_frame(ps) print "Updating frame to: %s \r\n" % ps if not self.wipe_started: self.wipe_appr_seed = ps self.wipe_ends[0] = self.aul.find_approach(ps, 0) print "wipe_end[0]: %s" % self.wipe_ends[0] (reachable, ik_goal) = self.aul.full_ik_check(self.wipe_ends[0]) if not reachable: rospy.loginfo( "Cannot find approach for initial wipe position, please try another" ) self.wt_log_out.publish( data= "Cannot find approach for initial wipe position, please try another" ) return None else: self.wipe_started = True rospy.loginfo("Received starting position for wiping action") self.wt_log_out.publish( data="Received starting position for wiping action") return None else: self.wipe_ends[1] = self.aul.find_approach(ps, 0) self.wipe_ends.reverse() (reachable, ik_goal) = self.aul.full_ik_check(self.wipe_ends[1]) if not reachable: rospy.loginfo( "Cannot find approach for final wipe position, please try another" ) self.wt_log_out.publish( data= "Cannot find approach for final wipe position, please try another" ) return None else: rospy.loginfo("Received End position for wiping action") self.wt_log_out.publish( data="Received End position for wiping action") self.aul.update_frame(self.wipe_ends[0]) self.wipe_ends[1].header.stamp = rospy.Time.now() self.tfl.waitForTransform(self.wipe_ends[1].header.frame_id, 'lh_utility_frame', rospy.Time.now(), rospy.Duration(3.0)) fin_in_start = self.tfl.transformPose('lh_utility_frame', self.wipe_ends[1]) ang = math.atan2(-fin_in_start.pose.position.z, -fin_in_start.pose.position.y) + (math.pi / 2) q_st_rot = transformations.quaternion_about_axis( ang, (1, 0, 0)) q_st_new = transformations.quaternion_multiply([ self.wipe_ends[0].pose.orientation.x, self.wipe_ends[0].pose.orientation.y, self.wipe_ends[0].pose.orientation.z, self.wipe_ends[0].pose.orientation.w ], q_st_rot) self.wipe_ends[0].pose.orientation.x = q_st_new[0] self.wipe_ends[0].pose.orientation.y = q_st_new[1] self.wipe_ends[0].pose.orientation.z = q_st_new[2] self.wipe_ends[0].pose.orientation.w = q_st_new[3] self.aul.update_frame(self.wipe_ends[1]) self.wipe_ends[0].header.stamp = rospy.Time.now() self.tfl.waitForTransform(self.wipe_ends[0].header.frame_id, 'lh_utility_frame', rospy.Time.now(), rospy.Duration(3.0)) start_in_fin = self.tfl.transformPose('lh_utility_frame', self.wipe_ends[0]) ang = math.atan2(start_in_fin.pose.position.z, start_in_fin.pose.position.y) + (math.pi / 2) q_st_rot = transformations.quaternion_about_axis( ang, (1, 0, 0)) q_st_new = transformations.quaternion_multiply([ self.wipe_ends[1].pose.orientation.x, self.wipe_ends[1].pose.orientation.y, self.wipe_ends[1].pose.orientation.z, self.wipe_ends[1].pose.orientation.w ], q_st_rot) self.wipe_ends[1].pose.orientation.x = q_st_new[0] self.wipe_ends[1].pose.orientation.y = q_st_new[1] self.wipe_ends[1].pose.orientation.z = q_st_new[2] self.wipe_ends[1].pose.orientation.w = q_st_new[3] self.aul.update_frame(self.wipe_appr_seed) appr = self.aul.find_approach(self.wipe_appr_seed, 0.15) appr.pose.orientation = self.wipe_ends[1].pose.orientation prepared = self.aul.move_arm_to(appr) if prepared: #self.aul.advance_to_contact() self.aul.blind_move(self.wipe_ends[1]) traj = self.aul.build_trajectory(self.wipe_ends[1], self.wipe_ends[0]) wipe_traj = self.aul.build_follow_trajectory(traj) self.aul.wait_for_stop() self.wipe_started = False return wipe_traj #self.wipe(wipe_traj) else: rospy.loginfo( "Failure reaching start point, please try again") self.wt_log_out.publish( data="Failure reaching start point, please try again") def wipe_move(self, traj_goal, passes=4): times = [] for i in range(len(traj_goal.trajectory.points)): times.append(traj_goal.trajectory.points[i].time_from_start) count = 0 while count < passes: traj_goal.trajectory.points.reverse() for i in range(len(times)): traj_goal.trajectory.points[i].time_from_start = times[i] traj_goal.trajectory.header.stamp = rospy.Time.now() assert traj_goal.trajectory.points[0] != [] self.aul.l_arm_follow_traj_client.send_goal(traj_goal) self.aul.l_arm_follow_traj_client.wait_for_result( rospy.Duration(20)) rospy.sleep(0.5) # Pause at end of swipe count += 1 rospy.loginfo("Done Wiping") self.wt_log_out.publish(data="Done Wiping") hfm_pose = self.aul.hand_frame_move(-0.15) self.aul.blind_move(hfm_pose) def broadcast_pose(self): broadcast_rate = rospy.Rate(10) while not rospy.is_shutdown(): self.pose_out.publish(self.aul.curr_pose()) broadcast_rate.sleep()
class ParticleFilter: """ The class that represents a Particle Filter ROS Node Attributes list: initialized: a Boolean flag to communicate to other class methods that initializaiton is complete base_frame: the name of the robot base coordinate frame (should be "base_link" for most robots) map_frame: the name of the map coordinate frame (should be "map" in most cases) odom_frame: the name of the odometry coordinate frame (should be "odom" in most cases) scan_topic: the name of the scan topic to listen to (should be "scan" in most cases) n_particles: the number of particles in the filter d_thresh: the amount of linear movement before triggering a filter update a_thresh: the amount of angular movement before triggering a filter update laser_max_distance: the maximum distance to an obstacle we should use in a likelihood calculation pose_listener: a subscriber that listens for new approximate pose estimates (i.e. generated through the rviz GUI) particle_pub: a publisher for the particle cloud laser_subscriber: listens for new scan data on topic self.scan_topic tf_listener: listener for coordinate transforms tf_broadcaster: broadcaster for coordinate transforms particle_cloud: a list of particles representing a probability distribution over robot poses current_odom_xy_theta: the pose of the robot in the odometry frame when the last filter update was performed. The pose is expressed as a list [x,y,theta] (where theta is the yaw) map: the map we will be localizing ourselves in. The map should be of type nav_msgs/OccupancyGrid """ def __init__(self): self.initialized = False # make sure we don't perform updates before everything is setup rospy.init_node( 'pf') # tell roscore that we are creating a new node named "pf" self.base_frame = "base_link" # the frame of the robot base self.map_frame = "map" # the name of the map coordinate frame self.odom_frame = "odom" # the name of the odometry coordinate frame self.scan_topic = "scan" # the topic where we will get laser scans from self.n_particles = 1000 # the number of particles to use self.n_angles = 20 # the number of angles to use from the range scan. Value contained in the interval [1,360] self.d_thresh = 0.2 # the amount of linear movement before performing an update self.a_thresh = math.pi / 6 # the amount of angular movement before performing an update self.laser_max_distance = 2.0 # maximum penalty to assess in the likelihood field model self.std_x = 1 # std of x self.std_y = 1 # std of y self.marker_multiplier = self.n_particles / 5 # Setup pubs and subs # pose_listener responds to selection of a new approximate robot location (for instance using rviz) self.pose_listener = rospy.Subscriber("initialpose", PoseWithCovarianceStamped, self.update_initial_pose) # publish the current particle cloud. This enables viewing particles in rviz. self.particle_pub = rospy.Publisher("particlecloud", PoseArray, queue_size=10) self.marker_pub = rospy.Publisher("markerpub", MarkerArray, queue_size=10) # laser_subscriber listens for data from the lidar self.laser_subscriber = rospy.Subscriber(self.scan_topic, LaserScan, self.scan_received) # enable listening for and broadcasting coordinate transforms self.tf_listener = TransformListener() self.tf_broadcaster = TransformBroadcaster() self.particle_cloud = [] self.current_odom_xy_theta = [] # request the map from the map server, the map should be of type nav_msgs/OccupancyGrid rospy.wait_for_service('static_map') get_map = rospy.ServiceProxy('static_map', GetMap) map = get_map().map # for now we have commented out the occupancy field initialization until you can successfully fetch the map self.occupancy_field = OccupancyField(map) self.initialized = True def update_robot_pose(self): """ Update the estimate of the robot's pose given the updated particles. There are two logical methods for this: (1): compute the mean pose (2): compute the most likely pose (i.e. the mode of the distribution) """ # first make sure that the particle weights are normalized self.normalize_particles() mean_x = sum( [particle.w * particle.x for particle in self.particle_cloud]) mean_y = sum( [particle.w * particle.y for particle in self.particle_cloud]) theta_x = np.mean([ particle.w * math.cos(particle.theta) for particle in self.particle_cloud ]) theta_y = np.mean([ particle.w * math.sin(particle.theta) for particle in self.particle_cloud ]) mean_theta = math.atan2(theta_y, theta_x) self.robot_pose = Particle(mean_x, mean_y, mean_theta).as_pose() def update_particles_with_odom(self, msg): """ Update the particles using the newly given odometry pose. The function computes the value delta which is a tuple (x,y,theta) that indicates the change in position and angle between the odometry when the particles were last updated and the current odometry. msg: this is not really needed to implement this, but is here just in case. """ new_odom_xy_theta = convert_pose_to_xy_and_theta(self.odom_pose.pose) # compute the change in x,y,theta since our last update if self.current_odom_xy_theta: old_odom_xy_theta = self.current_odom_xy_theta delta = (new_odom_xy_theta[0] - self.current_odom_xy_theta[0], new_odom_xy_theta[1] - self.current_odom_xy_theta[1], new_odom_xy_theta[2] - self.current_odom_xy_theta[2]) self.current_odom_xy_theta = new_odom_xy_theta else: self.current_odom_xy_theta = new_odom_xy_theta return print "delta: ", delta x, y = delta[0], delta[1] r = math.sqrt(x**2 + y**2) theta = np.arctan2(float(y), float(x)) phi = theta - old_odom_xy_theta[2] for particle in self.particle_cloud: particle.x += r * math.cos(phi + particle.theta) particle.y += r * math.sin(phi + particle.theta) particle.theta += delta[2] # particle.theta += phi + delta[2] # TODO: modify particles using delta # For added difficulty: Implement sample_motion_odometry (Prob Rob p 136) #wzc!!!!!!!!!!!!!???????????????????????????? def map_calc_range(self, x, y, theta): """ Difficulty Level 3: implement a ray tracing likelihood model... Let me know if you are interested """ # TODO: nothing unless you want to try this alternate likelihood model pass def resample_particles(self): """ Resample the particles according to the new particle weights. The weights stored with each particle should define the probability that a particular particle is selected in the resampling step. You may want to make use of the given helper function draw_random_sample. """ # make sure the distribution is normalized self.normalize_particles() # draw_random_sample self.particle_cloud = self.draw_random_sample( self.particle_cloud, [particle.w for particle in self.particle_cloud], self.n_particles) self.normalize_particles() def update_particles_with_laser(self, msg): """ Updates the particle weights in response to the scan contained in the msg """ test_angles = np.asarray(np.linspace(0, 360, self.n_angles + 1)[:-1], dtype=int) for angle in test_angles: r_min_d = msg.ranges[angle] # ignore when a scan finds nothing if r_min_d == 0.0: continue for particle in self.particle_cloud: ref_x = particle.x + r_min_d * math.cos(angle * np.pi / 180 + particle.theta) ref_y = particle.y + r_min_d * math.sin(angle * np.pi / 180 + particle.theta) p_min_d = self.occupancy_field.get_closest_obstacle_distance( ref_x, ref_y) particle.w *= math.exp(-p_min_d**2) @staticmethod def weighted_values(values, probabilities, size): """ Return a random sample of size elements from the set values with the specified probabilities values: the values to sample from (numpy.ndarray) probabilities: the probability of selecting each element in values (numpy.ndarray) size: the number of samples """ bins = np.add.accumulate(probabilities) return values[np.digitize(random_sample(size), bins)] @staticmethod def draw_random_sample(choices, probabilities, n): """ Return a random sample of n elements from the set choices with the specified probabilities choices: the values to sample from represented as a list probabilities: the prob weighted_valuesability of selecting each element in choices represented as a list n: the number of samples """ values = np.array(range(len(choices))) probs = np.array(probabilities) bins = np.add.accumulate(probs) inds = values[np.digitize(random_sample(n), bins)] samples = [] for i in inds: samples.append(deepcopy(choices[int(i)])) return samples def update_initial_pose(self, msg): """ Callback function to handle re-initializing the particle filter based on a pose estimate. These pose estimates could be generated by another ROS Node or could come from the rviz GUI """ xy_theta = convert_pose_to_xy_and_theta(msg.pose.pose) self.initialize_particle_cloud(xy_theta) self.fix_map_to_odom_transform(msg) def initialize_particle_cloud(self, xy_theta=None): """ Initialize the particle cloud. Arguments xy_theta: a triple consisting of the mean x, y, and theta (yaw) to initialize the particle cloud around. If this input is ommitted, the odometry will be used """ if xy_theta == None: xy_theta = convert_pose_to_xy_and_theta(self.odom_pose.pose) self.particle_cloud = [] (x, y, theta) = xy_theta # Create N particles centered around initial guess for i in range(self.n_particles): sample_x = x + randn() * self.std_x sample_y = y + randn() * self.std_y sample_theta = theta + uniform() * (2 * np.pi) self.particle_cloud.append( Particle(sample_x, sample_y, sample_theta)) self.normalize_particles() self.update_robot_pose() def normalize_particles(self): """ Make sure the particle weights define a valid distribution (i.e. sum to 1.0) """ total = sum([particle.w for particle in self.particle_cloud]) for particle in self.particle_cloud: particle.w /= total def publish_particles(self, msg): particles_conv = [] for p in self.particle_cloud: particles_conv.append(p.as_pose()) # actually send the message so that we can view it in rviz self.particle_pub.publish( PoseArray(header=Header(stamp=rospy.Time.now(), frame_id=self.map_frame), poses=particles_conv)) marker_array = [] for index, particle in enumerate(self.particle_cloud): marker = Marker(header=Header(stamp=rospy.Time.now(), frame_id=self.map_frame), pose=particle.as_pose(), type=0, scale=Vector3( x=particle.w * 2 * self.marker_multiplier, y=particle.w * 1 * self.marker_multiplier, z=particle.w * 4 * self.marker_multiplier), id=index, color=ColorRGBA(r=1, a=1)) marker_array.append(marker) self.marker_pub.publish(MarkerArray(markers=marker_array)) def scan_received(self, msg): """ This is the default logic for what to do when processing scan data. Feel free to modify this, however, I hope it will provide a good guide. The input msg is an object of type sensor_msgs/LaserScan """ if not (self.initialized): # wait for initialization to complete return if not (self.tf_listener.canTransform( self.base_frame, msg.header.frame_id, msg.header.stamp)): # need to know how to transform the laser to the base frame # this will be given by either Gazebo or neato_node return if not (self.tf_listener.canTransform(self.base_frame, self.odom_frame, msg.header.stamp)): # need to know how to transform between base and odometric frames # this will eventually be published by either Gazebo or neato_node return # calculate pose of laser relative to the robot base p = PoseStamped( header=Header(stamp=rospy.Time(0), frame_id=msg.header.frame_id)) #ZCW isnt laser in base frame alreay??? self.laser_pose = self.tf_listener.transformPose(self.base_frame, p) # find out where the robot thinks it is based on its odometry p = PoseStamped(header=Header(stamp=msg.header.stamp, frame_id=self.base_frame), pose=Pose()) self.odom_pose = self.tf_listener.transformPose(self.odom_frame, p) # store the the odometry pose in a more convenient format (x,y,theta) new_odom_xy_theta = convert_pose_to_xy_and_theta(self.odom_pose.pose) # print new_odom_xy_theta # print self.current_odom_xy_theta if not (self.particle_cloud): print "before initialize particle_cloud" # now that we have all of the necessary transforms we can update the particle cloud self.initialize_particle_cloud() # cache the last odometric pose so we can only update our particle filter if we move more than self.d_thresh or self.a_thresh self.current_odom_xy_theta = new_odom_xy_theta # update our map to odom transform now that the particles are initialized self.fix_map_to_odom_transform(msg) print "self.current_odom_xy_theta", self.current_odom_xy_theta elif (math.fabs(new_odom_xy_theta[0] - self.current_odom_xy_theta[0]) > self.d_thresh or math.fabs(new_odom_xy_theta[1] - self.current_odom_xy_theta[1]) > self.d_thresh or math.fabs(new_odom_xy_theta[2] - self.current_odom_xy_theta[2]) > self.a_thresh): # we have moved far enough to do an update! self.update_particles_with_odom(msg) # update based on odometry self.update_particles_with_laser(msg) # update based on laser scan self.update_robot_pose() # update robot's pose self.resample_particles( ) # resample particles to focus on areas of high density self.fix_map_to_odom_transform( msg ) # update map to odom transform now that we have new particles print "sum, length: ", (sum([ particle.w for particle in self.particle_cloud ]), len(self.particle_cloud)) # publish particles (so things like rviz can see them) self.publish_particles(msg) def fix_map_to_odom_transform(self, msg): """ This method constantly updates the offset of the map and odometry coordinate systems based on the latest results from the localizer """ (translation, rotation) = convert_pose_inverse_transform(self.robot_pose) p = PoseStamped(pose=convert_translation_rotation_to_pose( translation, rotation), header=Header(stamp=msg.header.stamp, frame_id=self.base_frame)) self.odom_to_map = self.tf_listener.transformPose(self.odom_frame, p) (self.translation, self.rotation) = convert_pose_inverse_transform(self.odom_to_map.pose) def broadcast_last_transform(self): """ Make sure that we are always broadcasting the last map to odom transformation. This is necessary so things like move_base can work properly. """ if not (hasattr(self, 'translation') and hasattr(self, 'rotation')): return self.tf_broadcaster.sendTransform(self.translation, self.rotation, rospy.get_rostime(), self.odom_frame, self.map_frame)
class OccupancyGridMapper: """ Implements simple occupancy grid mapping """ def __init__(self): cv2.namedWindow("map") #rospy.init_node("occupancy_grid_mapper") self.origin = [-10, -10] self.seq = 0 self.resolution = .1 self.n = 200 self.p_occ = .5 self.odds_ratio_hit = 3.0 self.odds_ratio_miss = .2 self.odds_ratios = (self.p_occ) / (1 - self.p_occ) * np.ones( (self.n, self.n)) rospy.Subscriber("scan", LaserScan, self.process_scan, queue_size=1) self.pub = rospy.Publisher("map", OccupancyGrid) self.tf_listener = TransformListener() def is_in_map(self, x_ind, y_ind): """ Return whether or not the given point is within the map boundaries """ return not (x_ind < self.origin[0] or x_ind > self.origin[0] + self.n * self.resolution or y_ind < self.origin[1] or y_ind > self.origin[1] + self.n * self.resolution) def process_scan(self, msg): """ Callback function for the laser scan messages """ if len(msg.ranges) != 360: # throw out scans that don't have all the laser data return # get pose according to the odometry p = PoseStamped(header=Header(stamp=msg.header.stamp, frame_id="base_link"), pose=Pose()) self.odom_pose = self.tf_listener.transformPose("odom", p) # convert the odom pose to the tuple (x,y,theta) self.odom_pose = OccupancyGridMapper.convert_pose_to_xy_and_theta( self.odom_pose.pose) for i in range(360): if msg.ranges[i] > 0.0 and msg.ranges[i] < 5.0: # TODO: draw a picture for this to understand it better map_x = self.odom_pose[0] + msg.ranges[i] * cos( i * pi / 180.0 + self.odom_pose[2]) map_y = self.odom_pose[1] + msg.ranges[i] * sin( i * pi / 180.0 + self.odom_pose[2]) x_detect = int((map_x - self.origin[0]) / self.resolution) y_detect = int((map_y - self.origin[1]) / self.resolution) u = (map_x - self.odom_pose[0], map_y - self.odom_pose[1]) magnitude = sqrt(u[0]**2 + u[1]**2) n_steps = max([1, int(ceil(magnitude / self.resolution))]) u_step = (u[0] / (n_steps - 1), u[1] / (n_steps - 1)) marked = set() for i in range(n_steps): curr_x = self.odom_pose[0] + i * u_step[0] curr_y = self.odom_pose[1] + i * u_step[1] if not (self.is_in_map(curr_x, curr_y)): break x_ind = int((curr_x - self.origin[0]) / self.resolution) y_ind = int((curr_y - self.origin[1]) / self.resolution) if x_ind == x_detect and y_ind == y_detect: break if not ((x_ind, y_ind) in marked): # odds ratio is updated according to the inverse sensor model self.odds_ratios[x_ind, y_ind] *= self.p_occ / ( 1 - self.p_occ) * self.odds_ratio_miss marked.add((x_ind, y_ind)) if self.is_in_map(map_x, map_y): # odds ratio is updated according to the inverse sensor model self.odds_ratios[x_detect, y_detect] *= self.p_occ / ( 1 - self.p_occ) * self.odds_ratio_hit self.seq += 1 # to save time, only publish the map every 10 scans that we process if self.seq % 10 == 0: # make occupancy grid map = OccupancyGrid() map.header.seq = self.seq self.seq += 1 map.header.stamp = msg.header.stamp map.header.frame_id = "map" # the name of the coordinate frame of the map map.info.origin.position.x = self.origin[0] map.info.origin.position.y = self.origin[1] map.info.width = self.n map.info.height = self.n map.info.resolution = self.resolution map.data = [ 0 ] * self.n**2 # map.data stores the n by n grid in row-major order for i in range(self.n): for j in range(self.n): idx = i + self.n * j # this implements row major order if self.odds_ratios[ i, j] < 1 / 5.0: # consider a cell free if odds ratio is low enough map.data[idx] = 0 elif self.odds_ratios[ i, j] > 5.0: # consider a cell occupied if odds ratio is high enough map.data[idx] = 100 else: # otherwise cell is unknown map.data[idx] = -1 self.pub.publish(map) # create the image from the probabilities so we can visualize using opencv im = np.zeros( (self.odds_ratios.shape[0], self.odds_ratios.shape[1], 3)) for i in range(im.shape[0]): for j in range(im.shape[1]): if self.odds_ratios[i, j] < 1 / 5.0: im[i, j, :] = 1.0 elif self.odds_ratios[i, j] > 5.0: im[i, j, :] = 0.0 else: im[i, j, :] = 0.5 # compute the index of the odometry pose so we can mark it with a circle x_odom_index = int( (self.odom_pose[0] - self.origin[0]) / self.resolution) y_odom_index = int( (self.odom_pose[1] - self.origin[1]) / self.resolution) # draw the circle cv2.circle(im, (y_odom_index, x_odom_index), 2, (255, 0, 0)) # display the image resized cv2.imshow("map", cv2.resize(im, (500, 500))) cv2.waitKey(20) @staticmethod def convert_pose_to_xy_and_theta(pose): """ Convert pose (geometry_msgs.Pose) to a (x,y,yaw) tuple """ orientation_tuple = (pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w) angles = euler_from_quaternion(orientation_tuple) return (pose.position.x, pose.position.y, angles[2]) def run(self): r = rospy.Rate(10) while not (rospy.is_shutdown()): r.sleep()
class ParticleFilter: """ The class that represents a Particle Filter ROS Node Attributes list: initialized: a Boolean flag to communicate to other class methods that initializaiton is complete base_frame: the name of the robot base coordinate frame (should be "base_link" for most robots) map_frame: the name of the map coordinate frame (should be "map" in most cases) odom_frame: the name of the odometry coordinate frame (should be "odom" in most cases) scan_topic: the name of the scan topic to listen to (should be "scan" in most cases) n_particles: the number of particles in the filter d_thresh: the amount of linear movement before triggering a filter update a_thresh: the amount of angular movement before triggering a filter update laser_max_distance: the maximum distance to an obstacle we should use in a likelihood calculation pose_listener: a subscriber that listens for new approximate pose estimates (i.e. generated through the rviz GUI) particle_pub: a publisher for the particle cloud laser_subscriber: listens for new scan data on topic self.scan_topic tf_listener: listener for coordinate transforms tf_broadcaster: broadcaster for coordinate transforms particle_cloud: a list of particles representing a probability distribution over robot poses current_odom_xy_theta: the pose of the robot in the odometry frame when the last filter update was performed. The pose is expressed as a list [x,y,theta] (where theta is the yaw) map: the map we will be localizing ourselves in. The map should be of type nav_msgs/OccupancyGrid """ def __init__(self): self.initialized = False # make sure we don't perform updates before everything is setup rospy.init_node( 'pf') # tell roscore that we are creating a new node named "pf" robot_name = rospy.get_namespace()[1:] self.base_frame = robot_name + "base_link" # the frame of the robot base self.map_frame = "map" # the name of the map coordinate frame self.odom_frame = robot_name + "odom" # the name of the odometry coordinate frame self.beacons_topic = "beacon_localization/distances/probabilistic" # the topic where we will get laser scans from TODO: filter parametrization self.n_particles = 500 # the number of particles to use self.d_thresh = 0.2 # the amount of linear movement before performing an update self.a_thresh = math.pi / 6 # the amount of angular movement before performing an update self.laser_max_distance = 2.0 # maximum penalty to assess in the likelihood field model self.sigma = 1 # guess for how inaccurate beacon scans are are in meters # Setup pubs and subs # pose_listener responds to selection of a new approximate robot location (for instance using rviz) self.pose_listener = rospy.Subscriber("initialpose", PoseWithCovarianceStamped, self.update_initial_pose) # publish the current particle cloud. This enables viewing particles in rviz. self.particle_pub = rospy.Publisher("bl/particlecloud", PoseArray, queue_size=10) self.pose_pub = rospy.Publisher('bl/pose', PoseWithCovarianceStamped, queue_size=10) self.marker_pub = rospy.Publisher("markers", MarkerArray, queue_size=10) # laser_subscriber listens for data from the lidar self.beacon_subscriber = rospy.Subscriber(self.beacons_topic, BeaconsScan, self.scan_received) # enable listening for and broadcasting coordinate transforms self.tf_listener = TransformListener() self.tf_broadcaster = TransformBroadcaster() self.particle_cloud = [] self.current_odom_xy_theta = [] self.robot_pose = None self.odom_pose = None self.receiver_pose = None self.initialized = True rospy.loginfo('Initialized particle filter') def update_robot_pose(self): """ Update the estimate of the robot's pose given the updated particles. Computed by taking the weighted average of poses. """ # first make sure that the particle weights are normalized self.normalize_particles() x = 0 y = 0 theta = 0 angles = [] x_var = 0 y_var = 0 for particle in self.particle_cloud: x += particle.x * particle.w y += particle.y * particle.w v = [ particle.w * math.cos(math.radians(particle.theta)), particle.w * math.sin(math.radians(particle.theta)) ] angles.append(v) theta = sum_vectors(angles) for particle in self.particle_cloud: x_var += (particle.x - x)**2 * particle.w y_var += (particle.y - y)**2 * particle.w print 'Position x, y, xvar, yvar', x, y, x_var, y_var covar = np.zeros((6, 6)) covar[0, 0] = x_var covar[1, 1] = y_var orientation_tuple = tf.transformations.quaternion_from_euler( 0, 0, theta) self.robot_pose = PoseWithCovarianceStamped( header=Header(stamp=rospy.Time.now(), frame_id=self.map_frame), pose=PoseWithCovariance(pose=Pose(position=Point(x=x, y=y), orientation=Quaternion( x=orientation_tuple[0], y=orientation_tuple[1], z=orientation_tuple[2], w=orientation_tuple[3])), covariance=covar.flatten())) def update_particles_with_odom(self, msg): """ Update the particles using the newly given odometry pose. The function computes the value delta which is a tuple (x,y,theta) that indicates the change in position and angle between the odometry when the particles were last updated and the current odometry. msg: this is not really needed to implement this, but is here just in case. """ new_odom_xy_theta = convert_pose_to_xy_and_theta(self.odom_pose.pose) # compute the change in x,y,theta since our last update if self.current_odom_xy_theta: old_odom_xy_theta = self.current_odom_xy_theta delta = (new_odom_xy_theta[0] - self.current_odom_xy_theta[0], new_odom_xy_theta[1] - self.current_odom_xy_theta[1], new_odom_xy_theta[2] - self.current_odom_xy_theta[2]) self.current_odom_xy_theta = new_odom_xy_theta else: self.current_odom_xy_theta = new_odom_xy_theta return for particle in self.particle_cloud: r1 = math.atan2(delta[1], delta[0]) - old_odom_xy_theta[2] d = math.sqrt((delta[0]**2) + (delta[1]**2)) particle.theta += r1 % 360 particle.x += d * math.cos(particle.theta) + normal(0, 0.1) particle.y += d * math.sin(particle.theta) + normal(0, 0.1) particle.theta += (delta[2] - r1 + normal(0, 0.1)) % 360 # For added difficulty: Implement sample_motion_odometry (Prob Rob p 136) def resample_particles(self): """ Resample the particles according to the new particle weights. The weights stored with each particle should define the probability that a particular particle is selected in the resampling step. You may want to make use of the given helper function draw_random_sample. """ # make sure the distribution is normalized self.normalize_particles() new_particles = [] for i in range(len(self.particle_cloud)): # resample the same # of particles choice = random_sample() # all the particle weights sum to 1 csum = 0 # cumulative sum for particle in self.particle_cloud: csum += particle.w if csum >= choice: # if the random choice fell within the particle's weight new_particles.append(deepcopy(particle)) break self.particle_cloud = new_particles def update_particles_with_beacons(self, msg): for particle in self.particle_cloud: total_probability = 1.0 for beacon in msg.beacons: distance = math.sqrt((beacon.pose.position.x - particle.x)**2 + (beacon.pose.position.y - particle.y)**2) total_probability *= gaussian(distance, self.sigma, beacon.distance) total_probability /= len(msg.beacons) particle.w = total_probability @staticmethod def weighted_values(values, probabilities, size): """ Return a random sample of size elements from the set values with the specified probabilities values: the values to sample from (numpy.ndarray) probabilities: the probability of selecting each element in values (numpy.ndarray) size: the number of samples """ bins = np.add.accumulate(probabilities) return values[np.digitize(random_sample(size), bins)] @staticmethod def draw_random_sample(choices, probabilities, n): """ Return a random sample of n elements from the set choices with the specified probabilities choices: the values to sample from represented as a list probabilities: the probability of selecting each element in choices represented as a list n: the number of samples """ values = np.array(range(len(choices))) probs = np.array(probabilities) bins = np.add.accumulate(probs) inds = values[np.digitize(random_sample(n), bins)] samples = [] for i in inds: samples.append(deepcopy(choices[int(i)])) return samples def update_initial_pose(self, msg): """ Callback function to handle re-initializing the particle filter based on a pose estimate. These pose estimates could be generated by another ROS Node or could come from the rviz GUI """ xy_theta = convert_pose_to_xy_and_theta(msg.pose.pose) self.initialize_particle_cloud(xy_theta) self.fix_map_to_odom_transform(msg) def initialize_particle_cloud(self, xy_theta=None): """ Initialize the particle cloud. Arguments xy_theta: a triple consisting of the mean x, y, and theta (yaw) to initialize the particle cloud around. If this input is ommitted, the odometry will be used """ if xy_theta == None: xy_theta = convert_pose_to_xy_and_theta(self.odom_pose.pose) rad = 1 # meters self.particle_cloud = [] self.particle_cloud.append( Particle(xy_theta[0], xy_theta[1], xy_theta[2])) for i in range(self.n_particles - 1): # initial facing of the particle theta = random.random() * 360 # compute params to generate x,y in a circle other_theta = random.random() * 360 radius = random.random() * rad # x => straight ahead x = radius * math.sin(other_theta) + xy_theta[0] y = radius * math.cos(other_theta) + xy_theta[1] particle = Particle(x, y, theta) self.particle_cloud.append(particle) self.normalize_particles() self.update_robot_pose() def normalize_particles(self): """ Make sure the particle weights define a valid distribution (i.e. sum to 1.0) """ tot_weight = sum([particle.w for particle in self.particle_cloud]) or 1 for particle in self.particle_cloud: particle.w = particle.w / tot_weight def publish_particles(self, msg): particles_conv = [] for p in self.particle_cloud: particles_conv.append(p.as_pose()) # actually send the message so that we can view it in rviz self.particle_pub.publish( PoseArray(header=Header(stamp=rospy.Time.now(), frame_id=self.map_frame), poses=particles_conv)) marker_array = [] for index, particle in enumerate(self.particle_cloud): marker = Marker(header=Header(stamp=rospy.Time.now(), frame_id=self.map_frame), pose=particle.as_pose(), type=0, scale=Vector3(x=particle.w * 2, y=particle.w * 1, z=particle.w * 5), id=index, color=ColorRGBA(r=1, a=1)) marker_array.append(marker) self.marker_pub.publish(MarkerArray(markers=marker_array)) self.pose_pub.publish(self.robot_pose) def scan_received(self, msg): """ This is the default logic for what to do when processing scan data. Feel free to modify this, however, I hope it will provide a good guide. The input msg is an object of type sensor_msgs/LaserScan """ rospy.loginfo('Scan received') if not self.initialized: # wait for initialization to complete rospy.loginfo('not initialized') return if not (self.tf_listener.canTransform( self.base_frame, msg.header.frame_id, msg.header.stamp)): return if not (self.tf_listener.canTransform(self.base_frame, self.odom_frame, msg.header.stamp)): self.tf_listener.waitForTransform(self.base_frame, self.odom_frame, msg.header.stamp, rospy.Duration(1.0 / 5)) # calculate pose of BLE receiver relative ot the robot base p = PoseStamped( header=Header(stamp=rospy.Time(0), frame_id=msg.header.frame_id)) self.receiver_pose = self.tf_listener.transformPose(self.base_frame, p) # find out where the robot thinks it is based on its odometry p = PoseStamped(header=Header(stamp=msg.header.stamp, frame_id=self.base_frame), pose=Pose()) # self.tf_listener.waitForTransform() self.odom_pose = self.tf_listener.transformPose(self.odom_frame, p) # store the the odometry pose in a more convenient format (x,y,theta) new_odom_xy_theta = convert_pose_to_xy_and_theta(self.odom_pose.pose) if not self.particle_cloud: # now that we have all of the necessary transforms we can update the particle cloud self.initialize_particle_cloud() # cache the last odometric pose so we can only update our particle filter if we move more than self.d_thresh or self.a_thresh self.current_odom_xy_theta = new_odom_xy_theta # update our map to odom transform now that the particles are initialized self.fix_map_to_odom_transform(msg) elif (math.fabs(new_odom_xy_theta[0] - self.current_odom_xy_theta[0]) > self.d_thresh or math.fabs(new_odom_xy_theta[1] - self.current_odom_xy_theta[1]) > self.d_thresh or math.fabs(new_odom_xy_theta[2] - self.current_odom_xy_theta[2]) > self.a_thresh): # we have moved far enough to do an update! self.update_particles_with_odom(msg) # update based on odometry self.update_particles_with_beacons( msg) # update based on laser scan self.update_robot_pose() # update robot's pose self.resample_particles( ) # resample particles to focus on areas of high density self.fix_map_to_odom_transform( msg ) # update map to odom transform now that we have new particles # publish particles (so things like rviz can see them) self.publish_particles(msg) def fix_map_to_odom_transform(self, msg): """ This method constantly updates the offset of the map and odometry coordinate systems based on the latest results from the localizer """ (translation, rotation) = convert_pose_inverse_transform(self.robot_pose.pose.pose) p = PoseStamped(pose=convert_translation_rotation_to_pose( translation, rotation), header=Header(stamp=msg.header.stamp, frame_id=self.base_frame)) self.odom_to_map = self.tf_listener.transformPose(self.odom_frame, p) (self.translation, self.rotation) = convert_pose_inverse_transform(self.odom_to_map.pose) def broadcast_last_transform(self): """ Make sure that we are always broadcasting the last map to odom transformation. This is necessary so things like move_base can work properly. """ if not (hasattr(self, 'translation') and hasattr(self, 'rotation')): return self.tf_broadcaster.sendTransform(self.translation, self.rotation, rospy.get_rostime(), self.odom_frame, self.map_frame)
class MarkerProcessor(object): def __init__(self, use_dummy_transform=False): rospy.init_node('star_center_positioning_node') if use_dummy_transform: self.odom_frame_name = "odom_dummy" else: self.odom_frame_name = "odom" self.marker_locators = {} self.add_marker_locator(MarkerLocator(0,(-6*12*2.54/100.0,0),0)) self.add_marker_locator(MarkerLocator(1,(0.0,0.0),pi)) self.add_marker_locator(MarkerLocator(2,(0.0,10*12*2.54/100.0),0)) self.add_marker_locator(MarkerLocator(3,(-6*12*2.54/100.0,6*12*2.54/100.0),0)) self.pose_correction = rospy.get_param('~pose_correction',0.0) self.marker_sub = rospy.Subscriber("ar_pose_marker", ARMarkers, self.process_markers) self.odom_sub = rospy.Subscriber("odom", Odometry, self.process_odom, queue_size=10) self.star_pose_pub = rospy.Publisher("STAR_pose",PoseStamped,queue_size=10) self.continuous_pose = rospy.Publisher("STAR_pose_continuous",PoseStamped,queue_size=10) self.tf_listener = TransformListener() self.tf_broadcaster = TransformBroadcaster() def add_marker_locator(self, marker_locator): self.marker_locators[marker_locator.id] = marker_locator def process_odom(self, msg): p = PoseStamped(header=Header(stamp=rospy.Time(0), frame_id=self.odom_frame_name), pose=msg.pose.pose) try: STAR_pose = self.tf_listener.transformPose("STAR", p) STAR_pose.header.stamp = msg.header.stamp self.continuous_pose.publish(STAR_pose) except Exception as inst: print "error is", inst def process_markers(self, msg): for marker in msg.markers: # do some filtering basd on prior knowledge # we know the approximate z coordinate and that all angles but yaw should be close to zero euler_angles = euler_from_quaternion((marker.pose.pose.orientation.x, marker.pose.pose.orientation.y, marker.pose.pose.orientation.z, marker.pose.pose.orientation.w)) angle_diffs = TransformHelpers.angle_diff(euler_angles[0],pi), TransformHelpers.angle_diff(euler_angles[1],0) print angle_diffs, marker.pose.pose.position.z if (marker.id in self.marker_locators and 3.2 <= marker.pose.pose.position.z <= 3.6 and fabs(angle_diffs[0]) <= .4 and fabs(angle_diffs[1]) <= .4): print "FOUND IT!" locator = self.marker_locators[marker.id] xy_yaw = list(locator.get_camera_position(marker)) xy_yaw[0] += self.pose_correction*cos(xy_yaw[2]) xy_yaw[1] += self.pose_correction*sin(xy_yaw[2]) orientation_tuple = quaternion_from_euler(0,0,xy_yaw[2]) pose = Pose(position=Point(x=-xy_yaw[0],y=-xy_yaw[1],z=0), orientation=Quaternion(x=orientation_tuple[0], y=orientation_tuple[1], z=orientation_tuple[2], w=orientation_tuple[3])) # TODO: use markers timestamp instead of now() (unfortunately, not populated currently by ar_pose) pose_stamped = PoseStamped(header=Header(stamp=rospy.Time.now(),frame_id="STAR"),pose=pose) try: offset, quaternion = self.tf_listener.lookupTransform("/base_link", "/base_laser_link", rospy.Time(0)) except Exception as inst: print "Error", inst return # TODO: use frame timestamp instead of now() pose_stamped_corrected = deepcopy(pose_stamped) pose_stamped_corrected.pose.position.x -= offset[0]*cos(xy_yaw[2]) pose_stamped_corrected.pose.position.y -= offset[0]*sin(xy_yaw[2]) self.star_pose_pub.publish(pose_stamped_corrected) self.fix_STAR_to_odom_transform(pose_stamped_corrected) def fix_STAR_to_odom_transform(self, msg): """ Super tricky code to properly update map to odom transform... do not modify this... Difficulty level infinity. """ (translation, rotation) = TransformHelpers.convert_pose_inverse_transform(msg.pose) p = PoseStamped(pose=TransformHelpers.convert_translation_rotation_to_pose(translation,rotation),header=Header(stamp=rospy.Time(),frame_id="base_link")) try: self.tf_listener.waitForTransform("odom","base_link",rospy.Time(),rospy.Duration(1.0)) except Exception as inst: print "whoops", inst return print "got transform" self.odom_to_STAR = self.tf_listener.transformPose("odom", p) (self.translation, self.rotation) = TransformHelpers.convert_pose_inverse_transform(self.odom_to_STAR.pose) def broadcast_last_transform(self): """ Make sure that we are always broadcasting the last map to odom transformation. This is necessary so things like move_base can work properly. """ if not(hasattr(self,'translation') and hasattr(self,'rotation')): return self.tf_broadcaster.sendTransform(self.translation, self.rotation, rospy.get_rostime(), self.odom_frame_name, "STAR") def run(self): r = rospy.Rate(10) while not rospy.is_shutdown(): self.broadcast_last_transform() r.sleep()
position, quaternion = transform.lookupTransform( "base_link", "odom", t) mapP = PoseStamped() mapP.header.stamp = rospy.get_rostime() mapP.header.frame_id = "odom" mapP.pose.position.x = position[0] mapP.pose.position.y = position[1] mapP.pose.position.z = position[2] mapP.pose.orientation.x = quaternion[0] mapP.pose.orientation.y = quaternion[1] mapP.pose.orientation.z = quaternion[2] mapP.pose.orientation.w = quaternion[3] # print(position) # print(quaternion) pNew = transform.transformPose("odom", p) print(pNew) pub.publish(p) rospy.loginfo(p) sys.exit() rate.sleep() else: #If it is to the left or equal degreesToTurn = degreesToTurn * -1 #The negative 1 is so it will turn to the left q = tf.transformations.quaternion_from_euler( 0, 0, radToTurn) p = PoseStamped() p.header.stamp = rospy.get_rostime() p.header.frame_id = 'base_link' p.pose.position.x = 0 p.pose.position.y = 0
class ParticleFilter: """ The class that represents a Particle Filter ROS Node Attributes list: initialized: a Boolean flag to communicate to other class methods that initializaiton is complete base_frame: the name of the robot base coordinate frame (should be "base_link" for most robots) map_frame: the name of the map coordinate frame (should be "map" in most cases) odom_frame: the name of the odometry coordinate frame (should be "odom" in most cases) scan_topic: the name of the scan topic to listen to (should be "scan" in most cases) n_particles: the number of particles in the filter d_thresh: the amount of linear movement before triggering a filter update a_thresh: the amount of angular movement before triggering a filter update laser_max_distance: the maximum distance to an obstacle we should use in a likelihood calculation pose_listener: a subscriber that listens for new approximate pose estimates (i.e. generated through the rviz GUI) particle_pub: a publisher for the particle cloud laser_subscriber: listens for new scan data on topic self.scan_topic tf_listener: listener for coordinate transforms tf_broadcaster: broadcaster for coordinate transforms particle_cloud: a list of particles representing a probability distribution over robot poses current_odom_xy_theta: the pose of the robot in the odometry frame when the last filter update was performed. The pose is expressed as a list [x,y,theta] (where theta is the yaw) map: the map we will be localizing ourselves in. The map should be of type nav_msgs/OccupancyGrid """ def __init__(self): self.initialized = False # make sure we don't perform updates before everything is setup rospy.init_node('pf') # tell roscore that we are creating a new node named "pf" self.base_frame = "base_link" # the frame of the robot base self.map_frame = "map" # the name of the map coordinate frame self.odom_frame = "odom" # the name of the odometry coordinate frame self.scan_topic = "scan" # the topic where we will get laser scans from self.n_particles = 300 # the number of particles to use self.d_thresh = 0.2 # the amount of linear movement before performing an update self.a_thresh = math.pi/6 # the amount of angular movement before performing an update self.laser_max_distance = 2.0 # maximum penalty to assess in the likelihood field model # TODO: define additional constants if needed #### DELETE BEFORE POSTING self.alpha1 = 0.2 self.alpha2 = 0.2 self.alpha3 = 0.2 self.alpha4 = 0.2 self.z_hit = 0.5 self.z_rand = 0.5 self.sigma_hit = 0.1 ##### DELETE BEFORE POSTING # Setup pubs and subs # pose_listener responds to selection of a new approximate robot location (for instance using rviz) self.pose_listener = rospy.Subscriber("initialpose", PoseWithCovarianceStamped, self.update_initial_pose) # publish the current particle cloud. This enables viewing particles in rviz. self.particle_pub = rospy.Publisher("particlecloud", PoseArray) # laser_subscriber listens for data from the lidar self.laser_subscriber = rospy.Subscriber(self.scan_topic, LaserScan, self.scan_received) # enable listening for and broadcasting coordinate transforms self.tf_listener = TransformListener() self.tf_broadcaster = TransformBroadcaster() self.particle_cloud = [] self.current_odom_xy_theta = [] # request the map # Difficulty level 2 rospy.wait_for_service("static_map") static_map = rospy.ServiceProxy("static_map", GetMap) try: map = static_map().map except: print "error receiving map" self.occupancy_field = OccupancyField(map) self.initialized = True def update_robot_pose(self): """ Update the estimate of the robot's pose given the updated particles. There are two logical methods for this: (1): compute the mean pose (2): compute the most likely pose (i.e. the mode of the distribution) """ """ Difficulty level 2 """ # first make sure that the particle weights are normalized self.normalize_particles() use_mean = True if use_mean: mean_x = 0.0 mean_y = 0.0 mean_theta = 0.0 theta_list = [] weighted_orientation_vec = np.zeros((2,1)) for p in self.particle_cloud: mean_x += p.x*p.w mean_y += p.y*p.w weighted_orientation_vec[0] += p.w*math.cos(p.theta) weighted_orientation_vec[1] += p.w*math.sin(p.theta) mean_theta = math.atan2(weighted_orientation_vec[1],weighted_orientation_vec[0]) self.robot_pose = Particle(x=mean_x,y=mean_y,theta=mean_theta).as_pose() else: weights = [] for p in self.particle_cloud: weights.append(p.w) best_particle = np.argmax(weights) self.robot_pose = self.particle_cloud[best_particle].as_pose() def update_particles_with_odom(self, msg): """ Implement a simple version of this (Level 1) or a more complex one (Level 2) """ new_odom_xy_theta = TransformHelpers.convert_pose_to_xy_and_theta(self.odom_pose.pose) if self.current_odom_xy_theta: old_odom_xy_theta = self.current_odom_xy_theta delta = (new_odom_xy_theta[0] - self.current_odom_xy_theta[0], new_odom_xy_theta[1] - self.current_odom_xy_theta[1], new_odom_xy_theta[2] - self.current_odom_xy_theta[2]) self.current_odom_xy_theta = new_odom_xy_theta else: self.current_odom_xy_theta = new_odom_xy_theta return # Implement sample_motion_odometry (Prob Rob p 136) # Avoid computing a bearing from two poses that are extremely near each # other (happens on in-place rotation). delta_trans = math.sqrt(delta[0]*delta[0] + delta[1]*delta[1]) if delta_trans < 0.01: delta_rot1 = 0.0 else: delta_rot1 = ParticleFilter.angle_diff(math.atan2(delta[1], delta[0]), old_odom_xy_theta[2]) delta_rot2 = ParticleFilter.angle_diff(delta[2], delta_rot1) # We want to treat backward and forward motion symmetrically for the # noise model to be applied below. The standard model seems to assume # forward motion. delta_rot1_noise = min(math.fabs(ParticleFilter.angle_diff(delta_rot1, 0.0)), math.fabs(ParticleFilter.angle_diff(delta_rot1, math.pi))); delta_rot2_noise = min(math.fabs(ParticleFilter.angle_diff(delta_rot2, 0.0)), math.fabs(ParticleFilter.angle_diff(delta_rot2, math.pi))); for sample in self.particle_cloud: # Sample pose differences delta_rot1_hat = ParticleFilter.angle_diff(delta_rot1, gauss(0, self.alpha1*delta_rot1_noise*delta_rot1_noise + self.alpha2*delta_trans*delta_trans)) delta_trans_hat = delta_trans - gauss(0, self.alpha3*delta_trans*delta_trans + self.alpha4*delta_rot1_noise*delta_rot1_noise + self.alpha4*delta_rot2_noise*delta_rot2_noise) delta_rot2_hat = ParticleFilter.angle_diff(delta_rot2, gauss(0, self.alpha1*delta_rot2_noise*delta_rot2_noise + self.alpha2*delta_trans*delta_trans)) # Apply sampled update to particle pose sample.x += delta_trans_hat * math.cos(sample.theta + delta_rot1_hat) sample.y += delta_trans_hat * math.sin(sample.theta + delta_rot1_hat) sample.theta += delta_rot1_hat + delta_rot2_hat def map_calc_range(self,x,y,theta): """ Difficulty Level 3: implement a ray tracing likelihood model... Let me know if you are interested """ pass def resample_particles(self): self.normalize_particles() values = np.empty(self.n_particles) probs = np.empty(self.n_particles) for i in range(len(self.particle_cloud)): values[i] = i probs[i] = self.particle_cloud[i].w new_particle_indices = ParticleFilter.weighted_values(values,probs,self.n_particles) new_particles = [] for i in new_particle_indices: idx = int(i) s_p = self.particle_cloud[idx] new_particles.append(Particle(x=s_p.x+gauss(0,.025),y=s_p.y+gauss(0,.025),theta=s_p.theta+gauss(0,.025))) self.particle_cloud = new_particles self.normalize_particles() # Difficulty level 1 def update_particles_with_laser(self, msg): """ Updates the particle weights in response to the scan contained in the msg """ laser_xy_theta = TransformHelpers.convert_pose_to_xy_and_theta(self.laser_pose.pose) for p in self.particle_cloud: adjusted_pose = (p.x+laser_xy_theta[0], p.y+laser_xy_theta[1], p.theta+laser_xy_theta[2]) # Pre-compute a couple of things z_hit_denom = 2*self.sigma_hit**2 z_rand_mult = 1.0/msg.range_max # This assumes quite a bit about the weights beforehand (TODO: could base this on p.w) new_prob = 1.0 # more agressive DEBUG, was 1.0 for i in range(0,len(msg.ranges),6): pz = 1.0 obs_range = msg.ranges[i] obs_bearing = i*msg.angle_increment+msg.angle_min if math.isnan(obs_range): continue if obs_range >= msg.range_max: continue # compute the endpoint of the laser end_x = p.x + obs_range*math.cos(p.theta+obs_bearing) end_y = p.y + obs_range*math.sin(p.theta+obs_bearing) z = self.occupancy_field.get_closest_obstacle_distance(end_x,end_y) if math.isnan(z): z = self.laser_max_distance else: z = z[0] # not sure why this is happening pz += self.z_hit * math.exp(-(z * z) / z_hit_denom) / (math.sqrt(2*math.pi)*self.sigma_hit) pz += self.z_rand * z_rand_mult new_prob += pz**3 p.w = new_prob pass @staticmethod def angle_normalize(z): """ convenience function to map an angle to the range [-pi,pi] """ return math.atan2(math.sin(z), math.cos(z)) @staticmethod def angle_diff(a, b): """ Calculates the difference between angle a and angle b (both should be in radians) the difference is always based on the closest rotation from angle a to angle b examples: angle_diff(.1,.2) -> -.1 angle_diff(.1, 2*math.pi - .1) -> .2 angle_diff(.1, .2+2*math.pi) -> -.1 """ a = ParticleFilter.angle_normalize(a) b = ParticleFilter.angle_normalize(b) d1 = a-b d2 = 2*math.pi - math.fabs(d1) if d1 > 0: d2 *= -1.0 if math.fabs(d1) < math.fabs(d2): return d1 else: return d2 @staticmethod def weighted_values(values, probabilities, size): """ Return a random sample of size elements form the set values with the specified probabilities values: the values to sample from (numpy.ndarray) probabilities: the probability of selecting each element in values (numpy.ndarray) size: the number of samples """ bins = np.add.accumulate(probabilities) return values[np.digitize(random_sample(size), bins)] def update_initial_pose(self, msg): """ Callback function to handle re-initializing the particle filter based on a pose estimate. These pose estimates could be generated by another ROS Node or could come from the rviz GUI """ xy_theta = TransformHelpers.convert_pose_to_xy_and_theta(msg.pose.pose) self.initialize_particle_cloud(xy_theta) self.fix_map_to_odom_transform(msg) def initialize_particle_cloud(self, xy_theta=None): """ Initialize the particle cloud. Arguments xy_theta: a triple consisting of the mean x, y, and theta (yaw) to initialize the particle cloud around. If this input is ommitted, the odometry will be used """ if xy_theta == None: xy_theta = TransformHelpers.convert_pose_to_xy_and_theta(self.odom_pose.pose) self.particle_cloud = [] for i in range(self.n_particles): self.particle_cloud.append(Particle(x=xy_theta[0]+gauss(0,.25),y=xy_theta[1]+gauss(0,.25),theta=xy_theta[2]+gauss(0,.25))) self.normalize_particles() self.update_robot_pose() """ Level 1 """ def normalize_particles(self): """ Make sure the particle weights define a valid distribution (i.e. sum to 1.0) """ z = 0.0 for p in self.particle_cloud: z += p.w for i in range(len(self.particle_cloud)): self.particle_cloud[i].w /= z def publish_particles(self, msg): particles_conv = [] for p in self.particle_cloud: particles_conv.append(p.as_pose()) # actually send the message so that we can view it in rviz self.particle_pub.publish(PoseArray(header=Header(stamp=rospy.Time.now(),frame_id=self.map_frame),poses=particles_conv)) def scan_received(self, msg): """ This is the default logic for what to do when processing scan data. Feel free to modify this, however, I hope it will provide a good guide. The input msg is an object of type sensor_msgs/LaserScan """ if not(self.initialized): # wait for initialization to complete return if not(self.tf_listener.canTransform(self.base_frame,msg.header.frame_id,msg.header.stamp)): # need to know how to transform the laser to the base frame # this will be given by either Gazebo or neato_node return if not(self.tf_listener.canTransform(self.base_frame,self.odom_frame,msg.header.stamp)): # need to know how to transform between base and odometric frames # this will eventually be published by either Gazebo or neato_node return # calculate pose of laser relative ot the robot base p = PoseStamped(header=Header(stamp=rospy.Time(0),frame_id=msg.header.frame_id)) self.laser_pose = self.tf_listener.transformPose(self.base_frame,p) # find out where the robot thinks it is based on its odometry p = PoseStamped(header=Header(stamp=msg.header.stamp,frame_id=self.base_frame), pose=Pose()) self.odom_pose = self.tf_listener.transformPose(self.odom_frame, p) # store the the odometry pose in a more convenient format (x,y,theta) new_odom_xy_theta = TransformHelpers.convert_pose_to_xy_and_theta(self.odom_pose.pose) if not(self.particle_cloud): # now that we have all of the necessary transforms we can update the particle cloud self.initialize_particle_cloud() # cache the last odometric pose so we can only update our particle filter if we move more than self.d_thresh or self.a_thresh self.current_odom_xy_theta = new_odom_xy_theta # update our map to odom transform now that the particles are initialized self.fix_map_to_odom_transform(msg) elif (math.fabs(new_odom_xy_theta[0] - self.current_odom_xy_theta[0]) > self.d_thresh or math.fabs(new_odom_xy_theta[1] - self.current_odom_xy_theta[1]) > self.d_thresh or math.fabs(new_odom_xy_theta[2] - self.current_odom_xy_theta[2]) > self.a_thresh): # we have moved far enough to do an update! self.update_particles_with_odom(msg) # update based on odometry self.update_particles_with_laser(msg) # update based on laser scan self.resample_particles() # resample particles to focus on areas of high density self.update_robot_pose() # update robot's pose self.fix_map_to_odom_transform(msg) # update map to odom transform now that we have new particles # publish particles (so things like rviz can see them) self.publish_particles(msg) def fix_map_to_odom_transform(self, msg): """ Super tricky code to properly update map to odom transform... do not modify this... Difficulty level infinity. """ (translation, rotation) = TransformHelpers.convert_pose_inverse_transform(self.robot_pose) p = PoseStamped(pose=TransformHelpers.convert_translation_rotation_to_pose(translation,rotation),header=Header(stamp=msg.header.stamp,frame_id=self.base_frame)) self.odom_to_map = self.tf_listener.transformPose(self.odom_frame, p) (self.translation, self.rotation) = TransformHelpers.convert_pose_inverse_transform(self.odom_to_map.pose) def broadcast_last_transform(self): """ Make sure that we are always broadcasting the last map to odom transformation. This is necessary so things like move_base can work properly. """ if not(hasattr(self,'translation') and hasattr(self,'rotation')): return self.tf_broadcaster.sendTransform(self.translation, self.rotation, rospy.get_rostime(), self.odom_frame, self.map_frame)
class DepthImageProcessing: """DepthImageProcessing Processes depth image and returns target object coordinates """ def __init__(self): image_topic = rospy.get_param("image_topic") depth_topic = image_topic.replace("color", "depth") depth_topic = depth_topic.replace("image_raw", "image_rect_raw") confidence_sub_topic = image_topic.replace("color", "confidence") depth_info_sub_topic = depth_topic.replace("image_raw", "camera_info") detections_topic = "/detected_objects_in_image" self.bridge = CvBridge() self.frame = None self.object_center = None self.camera_center = None self.depth = None self.target_coord_base = None self.intrinsics = None self.pix_grade = None self.tf_listener = TransformListener() self.bbox = None self.image_pub = rospy.Publisher( "visual_servoing_image", Image, queue_size=1000 ) self.depth_image_sub = rospy.Subscriber(depth_topic, Image, self.image_depth_cb) self.depth_confidence_sub = rospy.Subscriber( confidence_sub_topic, Image, self.depth_confidence_cb ) self.depth_info_sub = rospy.Subscriber( depth_info_sub_topic, CameraInfo, self.image_depth_info_cb ) self.image_sub = rospy.Subscriber(image_topic, Image, self.image_cb) self.detections_sub = rospy.Subscriber( detections_topic, BoundingBoxes, self.detections_cb ) def image_depth_cb(self, data): """ROS cb for image_depth arguments: data -- message of type sensor_msgs.image (depth) """ try: cv_image = self.bridge.imgmsg_to_cv2(data, data.encoding) line = "\r" if self.intrinsics and self.object_center is not None: # convert (x, y) to (row, col) for indexing target_pix = [self.object_center[1], self.object_center[0]] self.depth = cv_image[target_pix[0], target_pix[1]] line += "Depth at pixel(%3d, %3d): %7.1f(mm)." % ( target_pix[0], target_pix[1], self.depth, ) if self.pix_grade is not None: line += " Grade: %2d" % self.pix_grade if line: line += "\r" sys.stdout.write(line) sys.stdout.flush() except CvBridgeError as e: print(e) return except ValueError: return def depth_confidence_cb(self, data): try: cv_image = self.bridge.imgmsg_to_cv2(data, data.encoding) grades = np.bitwise_and(cv_image >> 4, 0x0F) if self.object_center is not None: # convert (x, y) to (row, col) for indexing target_pix = [self.object_center[0], self.object_center[1]] self.pix_grade = grades[target_pix[0], target_pix[1]] except CvBridgeError as e: print(e) return def image_depth_info_cb(self, camera_info): """ROS Callback for image_depth_info Arguments: camera_info -- message of type sensor_msgs.CameraInfo """ try: if self.intrinsics: return self.intrinsics = rs2.intrinsics() self.intrinsics.width = camera_info.width self.intrinsics.height = camera_info.height self.intrinsics.ppx = camera_info.K[2] self.intrinsics.ppy = camera_info.K[5] self.intrinsics.fx = camera_info.K[0] self.intrinsics.fy = camera_info.K[4] if camera_info.distortion_model == "plumb_bob": self.intrinsics.model = rs2.distortion.brown_conrady elif camera_info.distortion_model == "equidistant": self.intrinsics.model = rs2.distortion.kannala_brandt4 print(self.intrinsics) self.intrinsics.coeffs = [i for i in camera_info.D] except CvBridgeError as e: print(e) return def image_cb(self, image_data): """ROS Callback for image_topic Arguments: image_data -- message of type sensor_msgs.Image """ try: self.frame = self.bridge.imgmsg_to_cv2(image_data, "bgr8") # Color range for white chess piece (lower, upper) # chess_white = (np.array([15, 61, 33]), np.array([21, 139, 250])) # bbox = get_bbox_color(self.frame, WHITE_CHESS_PIECE_SIM) if bbox is not None: x, y, width, height = bbox # (x, y) where (0,0) is top left corner self.object_center = (round(x + width / 2), round(y + height / 2)) cv2.rectangle( self.frame, (x, y), (x + width, y + height), (255, 0, 0), 2 ) else: self.object_center = None if self.bbox is not None: x_min, x_max, y_min, y_max = ( self.bbox.xmin, self.bbox.xmax, self.bbox.ymin, self.bbox.ymax, ) self.object_center = ( round((x_min + x_max) / 2.0), round((y_min + y_max) / 2.0), ) self.frame = cv2.drawMarker( self.frame, self.object_center, (0, 255, 0), markerSize=20, thickness=2, line_type=cv2.MARKER_CROSS, ) height, width, dims = self.frame.shape self.camera_center = (round(width / 2), round(height / 2)) self.frame = cv2.drawMarker( self.frame, self.camera_center, (0, 0, 255), markerSize=30, thickness=2, line_type=cv2.MARKER_CROSS, ) self.image_pub.publish(self.bridge.cv2_to_imgmsg(self.frame, "bgr8")) except CvBridgeError as e: print(e) def detections_cb(self, bboxes): if len(bboxes.bounding_boxes) != 0: self.bbox = bboxes.bounding_boxes[0] def camera_to_base_coords(self, pose): camera_pose = PoseStamped() camera_pose.header.frame_id = "camera_link" camera_pose.pose.position.x = pose[2] camera_pose.pose.position.y = pose[0] camera_pose.pose.position.z = pose[1] camera_pose.pose.orientation.w = 1.0 # Neutral orientation world_pose = self.tf_listener.transformPose("/base_link", camera_pose) return world_pose def get_target_coord(self): return self.target_coord_base
class MirrorPointer(object): def __init__(self): self.tf = TransformListener() self.tfb = TransformBroadcaster() self.active = True self.head_pose = PoseStamped() self.goal_pub = rospy.Publisher('goal_pose', PoseStamped) rospy.Subscriber('/head_center', PoseStamped, self.head_pose_cb) rospy.Service('/point_mirror', PointMirror, self.point_mirror_cb) def head_pose_cb(self, msg): """Save update head pose, transforming to torso frame if necessary""" msg.header.stamp = rospy.Time(0) if not (msg.header.frame_id.lstrip('/') == 'torso_lift_link'): self.head_pose = self.tf.transformPose('/torso_lift_link', msg) else: self.head_pose = msg def get_current_mirror_pose(self): """Get the current pose of the mirror (hardcoded relative to tool frame""" mp = PoseStamped() mp.header.frame_id = "/r_gripper_tool_frame" mp.pose.position = Point(0.15, 0, 0) mp.pose.orientation = Quaternion(0,0,0,1) mirror_pose = self.tf.transformPose("torso_lift_link", mp) return mirror_pose def get_pointed_mirror_pose(self): """Get the pose of the mirror pointet at the goal location""" target_pt = PointStamped(self.head_pose.header, self.head_pose.pose.position) mp = self.get_current_mirror_pose() pu.aim_pose_to(mp, target_pt, (0,1,0)) return mp def trans_mirror_to_wrist(self, mp): """Get the wrist location from a mirror pose""" mp.header.stamp = rospy.Time(0) try: mp_in_mf = self.tf.transformPose('mirror',mp) except: return mp_in_mf.pose.position.x -= 0.15 try: wp = self.tf.transformPose('torso_lift_link',mp_in_mf) except: return return wp def head_pose_sensible(self, ps): """Set a bounding box on reasonably expected head poses""" if ((ps.pose.position.x < 0.35) or (ps.pose.position.x > 1.0) or (ps.pose.position.y < -0.2) or (ps.pose.position.y > 0.85) or (ps.pose.position.z < -0.3) or (ps.pose.position.z > 1) ): return False else: return True def point_mirror_cb(self, req): rospy.loginfo("Mirror Adjust Request Received") if not self.head_pose_sensible(self.head_pose): rospy.logwarn("Registered Head Position outside of expected region: %s" %self.head_pose) return PoseStamped() mp = self.get_pointed_mirror_pose() goal = self.trans_mirror_to_wrist(mp) goal.header.stamp = rospy.Time.now() resp = PointMirrorResponse() resp.wrist_pose = goal #print "Head Pose: " #print self.head_pose self.goal_pub.publish(goal) return resp def broadcast_mirror_tf(self): self.tfb.sendTransform((0.15,0,0), (0,0,0,1), rospy.Time.now(), "mirror", "r_gripper_tool_frame")
class obj_detector: options = { "model": "cfg/yolo.cfg", "load": "object_model/yolo.weights", "threshold": 0.3, "gpu": 0.2, "summary": None } def __init__(self, params): self.params = params self.match_images = {} ld = os.listdir('./object_images/') for fn in ld: if fn.split('.')[-1] != 'png' or len(fn.split('_')) != 3: continue on = fn.split('.')[0] img = cv2.imread('./object_images/' + fn) self.match_images[on] = img print 'matching objects loaded : ', self.match_images.keys() self.point_cloud = None self.object_id = 0 self.object_id_max = 999999 self.msg_idx = 0 self.msg_idx_max = 9999999 #Darknet self.gg = tensorflow.Graph() with self.gg.as_default() as g: self.tfnet = TFNet(self.options) self.classes = open('cfg/coco.names', 'r').readlines() if self.params['show_od']: cv2.startWindowThread() cv2.namedWindow('Object_detector') #ROS self.cvbridge = CvBridge() self.transform = TransformListener() self.transformer = Transformer(True, rospy.Duration(10.0)) self.RGB_TOPIC = params['rgb_topic'] self.Depth_TOPIC = params['depth_topic'] self.OBJ_TOPIC = params['obj_topic'] self.depth_sub = rospy.Subscriber(self.Depth_TOPIC, Image, self.callback_depth, queue_size=1) time.sleep(1) self.rgb_sub = rospy.Subscriber(self.RGB_TOPIC, Image, self.callback_image, queue_size=1) self.obj_pub = rospy.Publisher(self.OBJ_TOPIC, objs_array, queue_size=1) self.tttt = time.time() time.sleep(1) print('[DIP] Object Detector Module Ready!') def compare_hist(self, img1, img2): img1 = cv2.resize(img1, (32, 32)) img2 = cv2.resize(img2, (32, 32)) img1 = cv2.cvtColor(img1, cv2.COLOR_BGR2HSV) img2 = cv2.cvtColor(img2, cv2.COLOR_BGR2HSV) hist0 = cv2.calcHist([img1], [0, 1], None, [10, 16], [0, 180, 0, 256]) hist1 = cv2.calcHist([img2], [0, 1], None, [10, 16], [0, 180, 0, 256]) score = cv2.compareHist(hist0, hist1, 0) #method 0~6 #print score return score def callback_depth(self, msg): #return point cloud from depth image img = self.cvbridge.imgmsg_to_cv2(msg, 'passthrough') # Create point cloud from depth (for speed up) offset_x = -120 offset_y = -160 x_temp = -(np.tile( np.arange(img.shape[0]).reshape(img.shape[0], 1), (1, img.shape[1])) + offset_x) y_temp = -(np.tile( np.arange(img.shape[1]).reshape(1, img.shape[1]), (img.shape[0], 1)) + offset_y) fx = 525.0 * 0.54 fy = 525.0 * 0.54 z = img / 1000.0 x = (x_temp) * z / fx y = (y_temp) * z / fy cloud_np = np.zeros((img.shape[0], img.shape[1], 3)) cloud_np[:, :, 0] = z cloud_np[:, :, 1] = y cloud_np[:, :, 2] = x self.point_cloud = cloud_np.copy() def callback_image(self, msg): if self.point_cloud is None: print("No point cloud data") return None tic = time.time() img = self.cvbridge.imgmsg_to_cv2(msg, 'bgr8') img = cv2.resize(img, (320, 240)) detections = self.tfnet.return_predict(img) img_display = img.copy() objarray = objs_array() objarray.comm_delay = time.time() - self.tttt print '[DIP] Detection. time elapsed : ', time.time() - self.tttt self.tttt = time.time() objarray.header = msg.header objarray.header.stamp = rospy.Time.from_sec(time.time()) objarray.msg_idx = self.msg_idx self.msg_idx += 1 if self.msg_idx > self.msg_idx_max: self.msg_idx = 0 temp = [] objarray.header.frame_id = msg.header.frame_id temp_tt = 0 for i in range(len(detections)): obj = objs() obj.object_id = self.object_id self.object_id += 1 if self.object_id > self.object_id_max: self.object_id = 0 obj.person_id = -1 #unknown obj.person_name = '' obj.class_string = detections[i]['label'] obj.tags.append(detections[i]['label']) if obj.class_string == 'person': obj.tags.append('people') tlx = int(detections[i]['topleft']['y']) tly = int(detections[i]['topleft']['x']) brx = int(detections[i]['bottomright']['y']) bry = int(detections[i]['bottomright']['x']) x = (tlx + brx) / 2 y = (tly + bry) / 2 h = (brx - tlx) / 2 w = (bry - tly) / 2 obj.x = x obj.y = y obj.h = h obj.w = w obj.confidence = detections[i]['confidence'] crop = img[max(0, x - h):min(img.shape[0], x + h), max(0, y - w):min(img.shape[1], y + w)] ttiicc = time.time() max_score = -1 sub_class = None for mi in self.match_images.keys(): mi_spl = mi.split('_') mi_cls = mi_spl[0] mi_subcls = mi_spl[1] mi_idx = mi_spl[2] ob_cls = obj.class_string if mi_cls in self.class_reroute.keys(): mi_cls = self.class_reroute[mi_cls] if ob_cls in self.class_reroute.keys(): ob_cls = self.class_reroute[ob_cls] if ob_cls != mi_cls: continue scr = self.compare_hist(crop, self.match_images[mi]) #print mi, scr, if max_score < scr: max_score = scr sub_class = mi_subcls #print '' temp_tt += time.time() - ttiicc if sub_class is not None: obj.tags.append(sub_class) if self.params['show_od']: cv2.rectangle(img_display, (tly, tlx), (bry, brx), (0, 255, 0), 2) lbl = detections[i]['label'] if sub_class is None else sub_class cv2.putText(img_display, lbl, (tly, tlx - 8), cv2.FONT_HERSHEY_SIMPLEX, 0.3, color=(0, 0, 0), thickness=1) obj.cropped = self.cvbridge.cv2_to_imgmsg(crop, "bgr8") cropped_point = self.point_cloud[obj.x - obj.h:obj.x + obj.h, obj.y - obj.w:obj.y + obj.w] obj.cropped_cloud = self.cvbridge.cv2_to_imgmsg( cropped_point, encoding="passthrough") point_x = min(max(0, int(obj.x - 0.5 * obj.h)), 240) if self.params['use_loc']: pose_wrt_robot = self.get_pos_wrt_robot(point_x, obj.y, scan_len=obj.h, scan='point') if (pose_wrt_robot == 0).all(): continue if pose_wrt_robot[0] > 8.0: continue #max range = 10m?? obj.pose_wrt_robot.position.x = pose_wrt_robot[0] obj.pose_wrt_robot.position.y = pose_wrt_robot[1] obj.pose_wrt_robot.position.z = pose_wrt_robot[2] pose_wrt_map = self.get_loc(pose_wrt_robot)[0] obj.pose_wrt_map.position.x = pose_wrt_map[0] obj.pose_wrt_map.position.y = pose_wrt_map[1] obj.pose_wrt_map.position.z = pose_wrt_map[2] pose_wrt_odom = self.get_loc(pose_wrt_robot, target='odom')[0] obj.pose_wrt_odom.position.x = pose_wrt_odom[0] obj.pose_wrt_odom.position.y = pose_wrt_odom[1] obj.pose_wrt_odom.position.z = pose_wrt_odom[2] obj.valid_pose = 1 temp.append(obj) #print temp_tt objarray.objects = temp objarray.scene_rgb = msg objarray.scene_cloud = self.cvbridge.cv2_to_imgmsg( self.point_cloud, 'passthrough') if self.params['show_od']: cv2.imshow('Object_detector', cv2.resize(img_display, (640, 480))) self.obj_pub.publish(objarray) #print 'detection_process : ' , time.time()-tic def get_pos_wrt_robot(self, x, y, size=10, scan_len=50, scan='point'): #scan : point(around), vertical(line) if scan == 'point': x1 = min(240, max(0, x - size // 2)) x2 = min(240, max(0, x + size // 2)) y1 = min(320, max(0, y - size // 2)) y2 = min(320, max(0, y + size // 2)) roi = self.point_cloud[x1:x2, y1:y2] mask = roi[:, :, 0] > 0 masked = roi[mask] if masked.size == 0: return np.array([0, 0, 0]) mask = masked[:, 0] == masked[:, 0].min() masked = masked[mask] return masked[0] #self.point_cloud[x,y] else: xx1 = min(240, max(0, x - scan_len)) xx2 = min(240, max(0, x + scan_len)) roi = self.point_cloud[xx1:xx2, y - 2:y + 2, :] mask = roi[:, :, 0] > 0 masked = roi[mask] if masked.size == 0: return np.array([0, 0, 0]) mask = masked[:, 0] == masked[:, 0].min() masked = masked[mask] return masked[0] #self.point_cloud[x,y] def get_loc( self, p=np.array([0, 0, 0]), o=np.array([0, 0, 0, 1]), source='CameraTop_frame', target='map'): #pose = np.array([x,y,z]) : position w.r.t. robot pp = PoseStamped() pp.pose.position.x = p[0] pp.pose.position.y = p[1] pp.pose.position.z = p[2] pp.pose.orientation.x = o[0] pp.pose.orientation.y = o[1] pp.pose.orientation.z = o[2] pp.pose.orientation.w = o[3] #pp.header.stamp = rospy.get_rostime() pp.header.frame_id = source #'CameraDepth_frame' self.transform.waitForTransform(target, source, time=rospy.Time(), timeout=rospy.Duration(3.0)) asdf = self.transform.getLatestCommonTime(target, source) pp.header.stamp = asdf result = self.transform.transformPose(target, pp) result_p = np.array([ result.pose.position.x, result.pose.position.y, result.pose.position.z ]) result_o = np.array([ result.pose.orientation.x, result.pose.orientation.y, result.pose.orientation.z, result.pose.orientation.w ]) return result_p, result_o
class Shaving_manager(): def __init__(self): self.tf = TransformListener() self.jtarms = jttask_utils.jt_task_utils(self.tf) self.controller_switcher = ControllerSwitcher() ##### Subscribers #### self.disc_sub = rospy.Subscriber('wt_disc_shaving_step', Int8, self.disc_change_pose) self.disc_sub.impl.add_callback(self.cancel_goals, None) self.cont_sub = rospy.Subscriber('wt_cont_shaving_step', Point, self.cont_change_pose) self.cont_sub.impl.add_callback(self.cancel_goals, None) self.loc_sub = rospy.Subscriber('wt_shave_location', Int8, self.set_shave_pose) self.loc_sub.impl.add_callback(self.cancel_goals, None) rospy.Subscriber('wt_stop_shaving', Bool, self.stop_shaving) #### Services #### rospy.loginfo("Waiting for get_head_pose service") try: rospy.wait_for_service('/get_head_pose', 5.0) self.get_pose_proxy = rospy.ServiceProxy('/get_head_pose', GetHeadPose) rospy.loginfo("Found get_head_pose service") except: rospy.logwarn("get_head_pose service not available") rospy.loginfo("Waiting for ellipsoid_command service") try: rospy.wait_for_service('/ellipsoid_command', 5.0) self.ellipsoid_command_proxy = rospy.ServiceProxy('/ellipsoid_command', EllipsoidCommand) rospy.loginfo("Found ellipsoid_command service") except: rospy.logwarn("ellipsoid_command service not available") #### Publishers #### self.wt_log_out = rospy.Publisher('wt_log_out', String) self.test_out = rospy.Publisher('test_pose_1', PoseStamped) self.rezero_wrench = rospy.Publisher('/netft_gravity_zeroing/rezero_wrench', Bool) #### Data #### self.hand_offset = 0.195 self.angle_tool_offset = 0.03 self.inline_tool_offset = 0.085 self.selected_pose = 0 self.poses = { 0 : ["near_ear",0.], 1 : ["upper_cheek",0.], 2 : ["middle_cheek",0.], 3 : ["jaw_bone",0.], 4 : ["back_neck",0.], 5 : ["nose",0.], 6 : ["chin",0.], 7 : ["mouth_corner", 0.] } #self.ft_wrench = WrenchStamped() # self.force_unsafe = False # self.ft_activity_thresh = 3 # self.ft_safety_thresh = 12 # rospy.Subscriber('ft_data_pm_adjusted', WrenchStamped, self.get_ft_state) # rospy.Subscriber('/netft_gravity_zeroing/wrench_zeroed', WrenchStamped, self.get_ft_state) if POSTURE: rospy.sleep(3) print 'Sending Posture' self.jtarms.send_posture(posture='elbowup', arm=0) # def get_ft_state(self, ws): # self.ft_wrench = ws # self.ft_mag = math.sqrt(ws.wrench.force.x**2 + ws.wrench.force.y**2 + ws.wrench.force.z**2) # if self.ft_mag > self.ft_safety_thresh: # self.force_unsafe = True # if self.ft_mag > self.ft_activity_thresh: # self.ft_activity = rospy.Time.now() def stop_shaving(self, msg): self.cancel_goals(msg) rospy.loginfo("Stopping Shaving Behavior") self.wt_log_out.publish(data="Stopping Shaving Behavior") self.controller_switcher.carefree_switch('l','%s_arm_controller') def cancel_goals(self, msg): self.jtarms.ft_move_client.cancel_all_goals() #Stop what we're doing, as we're about to do something different, and stopping soon may be desired self.jtarms.ft_hold_client.cancel_all_goals() rospy.loginfo("Cancelling Active Shaving Goals") self.wt_log_out.publish(data="Cancelling Active Shaving Goals") def cont_change_pose(self, step): self.controller_switcher.carefree_switch('l','%s_cart') self.new_pose = True req = EllipsoidCommandRequest() req.change_latitude = int(-step.y) req.change_longitude = int(-step.x) req.change_height = int(step.z) print req print type(req.change_latitude) self.ellipsoid_command_proxy.call(req) rospy.loginfo("Moving Across Ellipsoid") self.wt_log_out.publish("Moving Across Ellipsoid") def disc_change_pose(self, step): self.new_pose = True self.selected_pose += step.data if self.selected_pose > 7: self.selected_pose = 7 self.wt_log_out.publish(data="Final position in sequence, there is no next position") return elif self.selected_pose < 0: self.selected_pose = 0 self.wt_log_out.publish(data="First position in sequence, there is no previous position") return rospy.loginfo("Moving to "+self.poses[self.selected_pose][0]) self.wt_log_out.publish(data="Moving to "+self.poses[self.selected_pose][0]) ellipse_pose = self.get_pose_proxy(self.poses[self.selected_pose][0], self.poses[self.selected_pose][1]).pose self.adjust_pose(ellipse_pose) def set_shave_pose(self, num): self.selected_pose = num.data rospy.loginfo("Moving to "+self.poses[self.selected_pose][0]) self.wt_log_out.publish(data="Moving to "+self.poses[self.selected_pose][0]) ellipse_pose = self.get_pose_proxy(self.poses[self.selected_pose][0], self.poses[self.selected_pose][1]).pose self.adjust_pose(ellipse_pose) def adjust_pose(self, ellipse_pose): self.controller_switcher.carefree_switch('l','%s_cart') self.jtarms.ft_move_client.cancel_all_goals() #Stop what we're doing, as we're about to do something different, and stopping soon may be desired self.jtarms.ft_hold_client.cancel_all_goals() #self.test_out.publish(ellipse_pose) print "New Position Received, should stop current action" self.tf.waitForTransform(ellipse_pose.header.frame_id, 'torso_lift_link', ellipse_pose.header.stamp, rospy.Duration(3.0)) torso_pose = self.tf.transformPose('torso_lift_link', ellipse_pose) if TOOL == 'inline': goal_pose = self.jtarms.pose_frame_move(torso_pose, -(self.hand_offset + self.inline_tool_offset), arm=0) approach_pose = self.jtarms.pose_frame_move(goal_pose, -0.15, arm=0) elif TOOL == '90': goal_pose = self.jtarms.pose_frame_move(torso_pose, 0.015) elif TOOL == '45': goal_pose = torso_pose approach_pose = self.jtarms.pose_frame_move(goal_pose, -0.15, arm=0) traj_to_approach = self.jtarms.build_trajectory(approach_pose, arm=0) traj_appr_to_goal = self.jtarms.build_trajectory(goal_pose, approach_pose, arm=0) self.ft_move(traj_to_approach) self.rezero_wrench.publish(data=True) self.ft_move(traj_appr_to_goal, ignore_ft=False) retreat_traj = self.jtarms.build_trajectory(approach_pose, arm=0) escape_traj = self.jtarms.build_trajectory(approach_pose, arm=0, steps=30) self.jtarms.ft_hold_client.send_goal(FtHoldGoal(2.,8,10,rospy.Duration(10)))#activity_thresh, z_unsafe, mag_unsafe, timeout print "Waiting for hold result" finished = self.jtarms.ft_hold_client.wait_for_result(rospy.Duration(0)) print "Finished before Timeout: %s" %finished print "Done Waiting" hold_result = self.jtarms.ft_hold_client.get_result() print "Hold Result:" print hold_result if hold_result.unsafe: self.ft_move(escape_traj) elif hold_result.timeout: self.ft_move(retreat_traj) def ft_move(self, traj, ft_thresh=2, ignore_ft=True): self.jtarms.ft_move_client.send_goal(FtMoveGoal(traj, ft_thresh, ignore_ft),feedback_cb=self.ft_move_feedback) finished_within_timeout = self.jtarms.ft_move_client.wait_for_result(rospy.Duration(0.25*len(traj))) if finished_within_timeout: result = self.jtarms.ft_move_client.get_result() if result.all_sent: rospy.loginfo("Full Trajectory Completed") self.wt_log_out.publish(data="Full Trajectory Completed") elif result.contact: rospy.loginfo("Stopped Trajectory upon contact") self.wt_log_out.publish(data="Stopped Trajectory upon contact") else: self.jtarms.ft_move_client.cancel_goal() rospy.loginfo("Failed to complete movement within timeout period.") self.wt_log_out.publish(data="Failed to complete movement within timeout period.") def ft_move_feedback(self, fdbk): pct = 100.*float(fdbk.current)/float(fdbk.total) #rospy.loginfo("Movement is %2.0f percent complete." %pct) self.wt_log_out.publish(data="Movement to %s is %2.0f percent complete." %(self.poses[self.selected_pose][0], pct)) #def hold_ft_aware(self, escape_pose, arm=0): # self.jtarms.force_stopped = False # self.force_unsafe = False # escape_traj = self.jtarms.build_trajectory(escape_pose) # time_since_activity = rospy.Duration(0) # self.ft_activity = rospy.Time.now() # while not (rospy.is_shutdown() or self.force_unsafe or time_since_activity>rospy.Duration(10) or self.new_pose): # time_since_activity = rospy.Time.now()-self.ft_activity # rospy.sleep(0.01) # if self.force_unsafe: # rospy.loginfo("Unsafe High Forces, moving away!") # self.wt_log_out.publish(data="Unsafe High Forces, moving away!") # self.jtarms.goal_pub[arm].publish(escape_pose) # return # if time_since_activity>rospy.Duration(10): # rospy.loginfo("10s of inactivity, moving away") # self.wt_log_out.publish(data="10s of inactivity, moving away") # if self.new_pose: # rospy.loginfo("New Pose Received") # self.wt_log_out.publish(data="New Pose Received") # self.new_pose = False # self.jtarms.send_traj(escape_traj) def test(self): goal_pose = PoseStamped() goal_pose.header.frame_id = 'torso_lift_link' goal_pose.header.stamp = rospy.Time.now() goal_pose.pose.position = Point(0.8, 0.3, 0.0) goal_pose.pose.orientation = Quaternion(1,0,0,0) approach_pose = self.jtarms.pose_frame_move(goal_pose, -0.15, arm=0) traj_to_approach = self.jtarms.build_trajectory(approach_pose, arm=0) traj_appr_to_goal = self.jtarms.build_trajectory(goal_pose, approach_pose, arm=0) raw_input("Move to approach pose") self.jtarms.send_traj(traj_to_approach) self.jtarms.force_stopped = False self.force_unsafe = False raw_input("Move to contact pose") self.jtarms.force_stopped = False self.force_unsafe = False self.jtarms.send_traj_to_contact(traj_appr_to_goal) raw_input("Hold under force constraints") self.jtarms.force_stopped = False self.force_unsafe = False self.hold_ft_aware(approach_pose, arm=0) rospy.loginfo("Finished Testing")
class Pick: def __init__(self): self.objects = [] self.iksvc = None self.object_bounding_boxes = dict() self.objectPoses = dict() self.graspService = rospy.ServiceProxy('grasp_service', GraspService) self.scene = moveit_commander.PlanningSceneInterface() self.robot = moveit_commander.RobotCommander() self.group = moveit_commander.MoveGroupCommander("left_arm") self.left_arm = baxter_interface.limb.Limb("left") self.limb_command = actionlib.SimpleActionClient( "/robot/left_velocity_trajectory_controller/follow_joint_trajectory", FollowJointTrajectoryAction) self.limb_command.wait_for_server() self.transformer = TransformListener() self.markers_publisher = rospy.Publisher("/grasp_markers", Marker) self.is_picking = False self.is_placing = False def moveToNeutral(self): trajectory = JointTrajectory() trajectory.header.stamp = rospy.Time.now() current_joints = self.left_arm.joint_angles() angles = dict( zip(self.left_arm.joint_names(), [0.0, -0.55, 0.0, 0.75, 0.0, 1.26, 0.0])) joints = self.interpolate(current_joints, angles) index = 0 for joint_dict in joints: point = JointTrajectoryPoint() point.time_from_start = rospy.rostime.Duration(0.015 * index) index += 1 for name, angle in joint_dict.iteritems(): if (index == 1): trajectory.joint_names.append(name) point.positions.append(angle) trajectory.points.append(point) trajectory.header.stamp = rospy.Time.now() + rospy.rostime.Duration( 1.0) goal = FollowJointTrajectoryGoal(trajectory=trajectory) rospy.loginfo("Moving left arm to neutral ") self.limb_command.send_goal(goal) self.limb_command.wait_for_result() def interpolate(self, start, end): joint_arrays = [] maxPoints = 2 for name in start.keys(): if name in end: diff = math.fabs(start[name] - end[name]) numPoints = diff / 0.01 if numPoints > maxPoints: maxPoints = int(numPoints) for i in range(maxPoints): t = float(i) / maxPoints joints = dict() for name in start.keys(): if name in end: current = (1 - t) * start[name] + t * end[name] joints[name] = current joint_arrays.append(joints) return joint_arrays def addBoundingBox(self, points, name): minX = sys.float_info.max minY = sys.float_info.max minZ = sys.float_info.max maxX = -sys.float_info.max maxY = -sys.float_info.max maxZ = -sys.float_info.max for point in points: if (point.x() > maxX): maxX = point.x() if (point.y() > maxY): maxY = point.y() if (point.z() > maxZ): maxZ = point.z() if (point.x() < minX): minX = point.x() if (point.y() < minY): minY = point.y() if (point.z() < minZ): minZ = point.z() dim_x = maxX - minX dim_y = maxY - minY dim_z = maxZ - minZ pose = PoseStamped() pose.header.frame_id = "/base" pose.pose.position.x = (maxX + minX) / 2.0 pose.pose.position.y = (maxY + minY) / 2.0 pose.pose.position.z = (maxZ + minZ) / 2.0 self.scene.add_box(name, pose, (dim_x, dim_y, dim_z)) def addBoundingBoxAtPose(self, name): width = 0.03 pose = self.objectPoses[name] pose.pose.position.z += 0.1 self.object_bounding_boxes[name] = dict() self.object_bounding_boxes[name]["scale"] = [width, width, 0.2] self.object_bounding_boxes[name]["pose"] = pose self.scene.add_box(name, pose, (width, width, 0.2)) def getPoseStampedFromPoseWithCovariance(self, pose): pose_stamped = PoseStamped() pose_stamped.header = copy.deepcopy(pose.header) pose_stamped.pose.position = copy.deepcopy(pose.pose.pose.position) pose_stamped.pose.position.z -= 0 pose_stamped.pose.orientation = copy.deepcopy( pose.pose.pose.orientation) now = rospy.Time.now() self.transformer.waitForTransform("/world", pose_stamped.header.frame_id, rospy.Time(), rospy.Duration(4, 0)) pose_stamped.header.stamp = self.transformer.getLatestCommonTime( "/world", pose_stamped.header.frame_id) transformedPose = self.transformer.transformPose( "/world", pose_stamped) return transformedPose def objectsCallback(self, msg): if self.is_picking or self.is_placing: return for object in self.objects: self.scene.remove_world_object(object) self.objects = [] self.objectPoses = dict() self.object_bounding_boxes = dict() for object in msg.objects: newPose = self.getPoseStampedFromPoseWithCovariance(object.pose) x = newPose.pose.position.x y = newPose.pose.position.y z = newPose.pose.position.z if x > 0.3 and y < 0.8 and y > -0.8 and z > -0.3: self.objects.append(object.type.key) self.objectPoses[object.type.key] = newPose self.addBoundingBoxAtPose(object.type.key) def burlapObjectRequestCallback(self, msg): if self.is_picking or self.is_placing: return object_name = msg.object.name object_id = msg.object.hashID if object_id not in self.objects: rospy.logerr("Object " + object_id + " (" + object_name + ") is not in detected objects ") object_str = "" for object in self.objects: object_str += ", " + str(object) rospy.logerr("Detected objects " + object_str) return rospy.loginfo("Getting grasp for object " + object_name) graspResponse = self.graspService(object_name) if not graspResponse.success: rospy.logerr("No grasps were found for object " + object_name) return rospy.loginfo("Finding a valid place pose") place_poses = self.getValidPlacePoses(msg.region, msg.header.frame_id, object_id) if len(place_poses) == 0: rospy.logerror("Place region is invalid") return rospy.loginfo("Attempting to pick up object " + object_name) self.is_picking = True self.moveToNeutral() pickSuccess = False try: pickSuccess = self.pick(object_name, object_id) except Exception as e: traceback.print_exc() if isinstance(e, TypeError): pickSuccess = True else: raise e finally: self.is_picking = False if not pickSuccess: rospy.logerr("Object pick up failed") return self.is_placing = True place_result = False try: for place_pose in place_poses: rospy.loginfo("Attempting to place object") if self.place(object_id, place_pose): break except Exception as e: traceback.print_exc() raise e finally: self.is_placing = False def objectRequestCallback(self, msg): if msg.data not in self.objects: rospy.logerr("Object " + msg.data + " is not in detected objects") return graspResponse = self.graspService(msg.data) if not graspResponse.success: rospy.logerr("No grasps were found for object " + msg.data) return self.group.set_start_state_to_current_state() robot.left_arm.pick(msg.data, graspResponse.grasps) def addTable(self): scene = moveit_commander.PlanningSceneInterface() p = PoseStamped() p.header.frame_id = "/base" p.pose.position.x = 0.35 p.pose.position.y = 0 p.pose.position.z = -0.75 scene.add_box("table", p, (2.1, 2.0, 1.0)) #0.35 def pick(self, object_name, object_id): self.group.detach_object() graspResponse = self.graspService(object_name) if not graspResponse.success: rospy.logerr("No grasps were found for object " + object_name + " with id: " + object_id) return self.group.set_planning_time(20) self.group.set_start_state_to_current_state() grasps = self.setGrasps(object_id, graspResponse.grasps) self.publishMarkers(grasps, object_name) result = self.group.pick(object_id, grasps * 5) return result def place(self, object_id, place_pose): goal_pose = copy.deepcopy(self.objectPoses[object_id]) goal_pose.pose.position.x = place_pose.pose.position.x goal_pose.pose.position.y = place_pose.pose.position.y result = self.group.place(object_id, goal_pose) return result def getValidPlacePoses(self, move_region, frame_id, object_id): rospy.loginfo("Finding valid open collision region") open_collision_region = self.getOpenCollisionRegion(move_region) radius = 0.05 if object_id in self.object_bounding_boxes: bounding_box = self.object_bounding_boxes[object_id] bb_width = bounding_box["scale"][0] bb_depth = bounding_box["scale"][1] bb_height = bounding_box["scale"][2] radius = math.sqrt(bb_width * bb_width + bb_depth * bb_depth) collision_map = self.getCollisionMap(open_collision_region, object_id, radius) unnocupied_cells = self.getUnnocupiedCells(collision_map) solved_joints = None random_place = None place_poses = [] while len(place_poses) == 0: rospy.loginfo("number of unnocupied places to try: " + str(len(unnocupied_cells))) if random_place != None: unnocupied_cells.remove(random_place) if len(unnocupied_cells) == 0: return None random_place = random.choice(unnocupied_cells) for i in range(36): #rospy.loginfo("unnocupied_cells: " + str(unnocupied_cells)) place_pose = PoseStamped() place_pose.header.frame_id = "world" place_pose.pose.position.x = 0.7 + random_place[ 0] / 1000.0 #convert back to meters place_pose.pose.position.y = 0.3 + random_place[1] / 1000.0 quat = quaternion_from_euler(0, math.pi / 2.0, i * 2.0 * math.pi / 36.0) place_pose.pose.orientation.x = quat[0] place_pose.pose.orientation.y = quat[1] place_pose.pose.orientation.z = quat[2] place_pose.pose.orientation.w = quat[3] place_poses.append(place_pose) return place_poses def getUnnocupiedCells(self, collision_map): unnocupied_cells = [] for y in range(len(collision_map)): for x in range(len(collision_map[y])): if (collision_map[y][x] == 0): unnocupied_cells.append([x, y]) return unnocupied_cells def getCollisionMap(self, collision_region, object_id, radius): distance_from_edge = copy.deepcopy(collision_region) for row in range(len(collision_region)): for column in range(len(collision_region[row])): occupancy = collision_region[row][column] if (occupancy > 0): for column_prime in range(int(column - radius), int(math.ceil(column + radius))): if column_prime >= 0 and column_prime < len( collision_region[row]): distance = math.fabs(column_prime - column) distance_from_edge[row][column_prime] = max( radius - distance, 0) reduced_collision_region = copy.deepcopy(distance_from_edge) for row in range(len(distance_from_edge)): for column in range(len(distance_from_edge[row])): distance = distance_from_edge[row][column] if (distance > 0): for row_prime in range(int(row - radius), int(math.ceil(row + radius))): if row_prime >= 0 and row_prime < len( distance_from_edge): column_distance = radius - distance_from_edge[row][ column] row_distance = math.fabs(row_prime - row) distance_squared = column_distance * column_distance + row_distance * row_distance distance = math.sqrt(distance_squared) reduced_collision_region[column][row_prime] = max( radius - distance, 0) #full_collision_map = copy.deepcopy(collision_region) #for object in self.objects: # if object != object_id: # bounding_box = self.object_bounding_boxes[object] return [[min(1, value) for value in row] for row in reduced_collision_region] def getOpenCollisionRegion(self, move_region): region_width = move_region.scale.x * 100 #convert meters into cm squares region_height = move_region.scale.y * 100 region_x = move_region.origin.x region_y = move_region.origin.y validity_function = { moveRegion.SHAPE_SQUARE: lambda x, y: math.fabs(x - region_x) <= region_width / 2.0 and math .fabs(y - region_y) <= region_height, moveRegion.SHAPE_CIRCLE: lambda x, y: (x - region_x) * (x - region_x) / region_width + (y - region_y) * (y - region_y) / region_height <= 1.0 } isValid = validity_function[move_region.shape] return [[int(isValid(x, y)) for x in range(100)] for y in range(100)] def setGrasps(self, name, grasps): pose = self.objectPoses[name] correctedGrasps = [] index = 0 for grasp in grasps: newGrasp = copy.deepcopy(grasp) newGrasp.id = str(index) index += 1 newGrasp.pre_grasp_posture.header.stamp = rospy.Time(0) newGrasp.grasp_posture.header.stamp = rospy.Time(0) newGrasp.grasp_pose.header.frame_id = 'world' newGrasp.grasp_pose.pose.position.x += pose.pose.position.x newGrasp.grasp_pose.pose.position.y += pose.pose.position.y newGrasp.grasp_pose.pose.position.z += pose.pose.position.z newGrasp.grasp_quality = 1.0 correctedGrasps.append(newGrasp) return correctedGrasps def publishMarkers(self, grasps, object_name): for grasp in grasps: marker = self.getMarker(grasp, object_name) self.markers_publisher.publish(marker) def getMarker(self, grasp, object_name): marker = Marker() marker.id = int(grasp.id) marker.header = grasp.grasp_pose.header marker.header.frame_id = grasp.grasp_pose.header.frame_id marker.pose = grasp.grasp_pose.pose marker.ns = object_name + "_grasp_" marker.lifetime.secs = 1 marker.action = 0 marker.color.r = 1 marker.color.g = 1 marker.color.b = 1 marker.color.a = 1 marker.scale.x = .1 marker.scale.y = .1 marker.scale.z = .1 return marker def solveIK(self, pose, limb): ns = "/ExternalTools/" + limb + "/PositionKinematicsNode/IKService" if self.iksvc == None: rospy.wait_for_service(ns, 5.0) self.iksvc = rospy.ServiceProxy(ns, SolvePositionIK) ikreq = SolvePositionIKRequest() hdr = Header(stamp=rospy.Time.now(), frame_id='base') goalPose = PoseStamped(header=hdr, pose=pose) ikreq.pose_stamp.append(goalPose) try: rospy.loginfo(ikreq) resp = self.iksvc(ikreq) except (rospy.ServiceException, rospy.ROSException), e: rospy.logerr("Service call failed: %s" % (e, )) return 1 if (resp.isValid[0]): limb_joints = dict( zip(resp.joints[0].name, resp.joints[0].position)) return limb_joints else: rospy.logwarn("INVALID POSE - No Valid Joint Solution Found.") return None
class ArmByFtWrist(object): def __init__(self): rospy.loginfo("Initializing...") # Node Hz rate self.rate = 10 self.rate_changed = False self.send_goals = False # Limits self.min_x = 0.0 self.max_x = 0.6 self.min_y = -0.5 self.max_y = -0.05 self.min_z = -0.2 self.max_z = 0.2 # Force stuff self.fx_scaling = 0.0 self.fy_scaling = 0.0 self.fz_scaling = 0.0 self.fx_deadband = 0.0 self.fy_deadband = 0.0 self.fz_deadband = 0.0 # Torque stuff self.tx_scaling = 0.0 self.ty_scaling = 0.0 self.tz_scaling = 0.0 self.tx_deadband = 0.0 self.ty_deadband = 0.0 self.tz_deadband = 0.0 self.axis_force = "zxy" self.sign_force = "+++" self.axis_torque = "zxy" self.sign_torque = "+++" # Signs adjusting fx, fy, fz, tx(roll), ty(pitch), tz(yaw) # for the left hand of reemc right now # And axis flipping self.frame_fixer = WrenchFixer(1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 'z', 'x', 'y', 'z', 'x', 'y') self.goal_frame_id = "arm_right_tool_link" self.global_frame_id = "world" self.wbc_goal_topic = "/whole_body_kinematic_controler/arm_right_tool_link_goal" self.pose_to_follow_topic = "/pose_to_follow" self.debug_topic = "/force_torqued_pose" self.force_torque_topic = "/right_wrist_ft" # All the previous params will be reset if there are parameters in the # param server self.dyn_rec_srv = Server(HandshakeConfig, self.dyn_rec_callback) # Goal to send to WBC # TODO: make this prettier self.tf_l = TransformListener() rospy.sleep(3.0) # Let the subscriber to its job self.initial_pose = self.get_initial_pose() self.tf_l = None self.current_pose = self.initial_pose self.last_pose_to_follow = deepcopy(self.current_pose) if self.goal_frame_id[0] == '/': self.goal_frame_id = self.goal_frame_id[1:] self.pose_pub = rospy.Publisher(self.wbc_goal_topic, PoseStamped, queue_size=1) # Safe debugging goal self.debug_pose_pub = rospy.Publisher(self.debug_topic, PoseStamped, queue_size=1) # Goal to follow self.follow_sub = rospy.Subscriber(self.pose_to_follow_topic, PoseStamped, self.follow_pose_cb, queue_size=1) # Sensor input self.last_wrench = None self.wrench_sub = rospy.Subscriber(self.force_torque_topic, WrenchStamped, self.wrench_cb, queue_size=1) rospy.loginfo("Done initializing.") def dyn_rec_callback(self, config, level): """ :param config: :param level: :return: """ rospy.loginfo("Received reconf call: " + str(config)) # Node Hz rate if self.rate != config['rate']: self.rate_changed = True self.rate = config['rate'] self.send_goals = config['send_goals'] # Limits self.min_x = config['min_x'] self.max_x = config['max_x'] self.min_y = config['min_y'] self.max_y = config['max_y'] self.min_z = config['min_z'] self.max_z = config['max_z'] # Force stuff self.fx_scaling = config['fx_scaling'] self.fy_scaling = config['fy_scaling'] self.fz_scaling = config['fz_scaling'] self.fx_deadband = config['fx_deadband'] self.fy_deadband = config['fy_deadband'] self.fz_deadband = config['fz_deadband'] # Torque stuff self.tx_scaling = config['tx_scaling'] self.ty_scaling = config['ty_scaling'] self.tz_scaling = config['tz_scaling'] self.tx_deadband = config['tx_deadband'] self.ty_deadband = config['ty_deadband'] self.tz_deadband = config['tz_deadband'] # Fixing transform stuff self.axis_force = config['axis_force'] self.sign_force = config['sign_force'] self.axis_torque = config['axis_torque'] self.sign_torque = config['sign_torque'] if config['goal_frame_id'][0] == '/': config['goal_frame_id'] = config['goal_frame_id'][1:] if config['goal_frame_id'] != self.goal_frame_id: self.goal_frame_id = config['goal_frame_id'] if config['global_frame_id'][0] == '/': config['global_frame_id'] = config['global_frame_id'][1:] if config['global_frame_id'] != self.global_frame_id: self.global_frame_id = config['global_frame_id'] if self.wbc_goal_topic != config["wbc_goal_topic"]: self.wbc_goal_topic = config["wbc_goal_topic"] self.pose_pub = rospy.Publisher(self.wbc_goal_topic, PoseStamped, queue_size=1) if self.debug_topic != config["debug_topic"]: self.debug_topic = config["debug_topic"] self.debug_pose_pub = rospy.Publisher(self.debug_topic, PoseStamped, queue_size=1) if self.force_torque_topic != config["force_torque_topic"]: self.force_torque_topic = config["force_torque_topic"] self.wrench_sub = rospy.Subscriber(self.force_torque_topic, WrenchStamped, self.wrench_cb, queue_size=1) if self.pose_to_follow_topic != config["pose_to_follow_topic"]: self.pose_to_follow_topic = config["pose_to_follow_topic"] self.follow_sub = rospy.Subscriber(self.pose_to_follow_topic, PoseStamped, self.follow_pose_cb, queue_size=1) args = [] for sign in self.sign_force: if sign == '+': args.append(1.0) else: args.append(-1.0) for sign in self.sign_torque: if sign == '+': args.append(1.0) else: args.append(-1.0) args.extend([self.axis_force[0], self.axis_force[1], self.axis_force[2], self.axis_torque[0], self.axis_torque[1], self.axis_torque[2]]) self.frame_fixer = WrenchFixer(*args) return config def follow_pose_cb(self, data): if data.header.frame_id[0] == '/': frame_id = data.header.frame_id[1:] else: frame_id = data.header.frame_id if frame_id != self.global_frame_id: rospy.logwarn( "Poses to follow should be in frame " + self.global_frame_id + " and is in " + str(frame_id) + ", transforming into " + self.global_frame_id + "...") world_ps = self.transform_pose_to_world( data.pose, from_frame=data.header.frame_id) ps = PoseStamped() ps.header.stamp = data.header.stamp ps.header.frame_id = self.global_frame_id ps.pose = world_ps self.last_pose_to_follow = ps else: self.last_pose_to_follow = data def transform_pose_to_world(self, pose, from_frame="arm_right_tool_link"): ps = PoseStamped() ps.header.stamp = self.tf_l.getLatestCommonTime( self.global_frame_id, from_frame) ps.header.frame_id = "/" + from_frame # TODO: check pose being PoseStamped or Pose ps.pose = pose transform_ok = False while not transform_ok and not rospy.is_shutdown(): try: world_ps = self.tf_l.transformPose(self.global_frame_id, ps) transform_ok = True return world_ps except ExtrapolationException as e: rospy.logwarn( "Exception on transforming pose... trying again \n(" + str(e) + ")") rospy.sleep(0.05) ps.header.stamp = self.tf_l.getLatestCommonTime( self.global_frame_id, from_frame) def wrench_cb(self, data): self.last_wrench = data def get_initial_pose(self): zero_pose = Pose() zero_pose.orientation.w = 1.0 current_pose = self.transform_pose_to_world( zero_pose, from_frame=self.goal_frame_id) return current_pose def get_abs_total_force(self, wrench_msg): f = wrench_msg.wrench.force return abs(f.x) + abs(f.y) + abs(f.z) def get_abs_total_torque(self, wrench_msg): t = wrench_msg.wrench.torque return abs(t.x) + abs(t.y) + abs(t.z) def run(self): rospy.loginfo( "Waiting for first wrench message on: " + str(self.wrench_sub.resolved_name)) while not rospy.is_shutdown() and self.last_wrench is None: rospy.sleep(0.2) r = rospy.Rate(self.rate) rospy.loginfo("Node running!") while not rospy.is_shutdown(): # To change the node rate if self.rate_changed: r = rospy.Rate(self.rate) self.rate_changed = False self.follow_pose_with_admitance_by_ft() r.sleep() def follow_pose_with_admitance_by_ft(self): fx, fy, fz = self.get_force_movement() rospy.loginfo( "fx, fy, fz: " + str((round(fx, 3), round(fy, 3), round(fz, 3)))) ps = Pose() if abs(fx) > self.fx_deadband: ps.position.x = (fx * self.fx_scaling) * self.frame_fixer.fx if abs(fy) > self.fy_deadband: ps.position.y = (fy * self.fy_scaling) * self.frame_fixer.fy if abs(fz) > self.fz_deadband: ps.position.z = (fz * self.fz_scaling) * self.frame_fixer.fz tx, ty, tz = self.get_torque_movement() rospy.loginfo( "tx, ty, tz: " + str((round(tx, 3), round(ty, 3), round(tz, 3)))) roll = pitch = yaw = 0.0 if abs(tx) > self.tx_deadband: roll += (tx * self.tx_scaling) * self.frame_fixer.tx if abs(ty) > self.ty_deadband: pitch += (ty * self.ty_scaling) * self.frame_fixer.ty if abs(tz) > self.tz_deadband: yaw += (tz * self.tz_scaling) * self.frame_fixer.tz q = quaternion_from_euler(roll, pitch, yaw) ps.orientation = Quaternion(*q) o = self.last_pose_to_follow.pose.orientation r_lastp, p_lastp, y_lastp = euler_from_quaternion([o.x, o.y, o.z, o.w]) final_roll = r_lastp + roll final_pitch = p_lastp + pitch final_yaw = y_lastp + yaw self.current_pose.pose.orientation = Quaternion(*quaternion_from_euler(final_roll, final_pitch, final_yaw)) self.current_pose.pose.position.x = self.last_pose_to_follow.pose.position.x + \ ps.position.x self.current_pose.pose.position.y = self.last_pose_to_follow.pose.position.y + \ ps.position.y self.current_pose.pose.position.z = self.last_pose_to_follow.pose.position.z + \ ps.position.z self.current_pose.pose.position.x = self.sanitize(self.current_pose.pose.position.x, self.min_x, self.max_x) self.current_pose.pose.position.y = self.sanitize(self.current_pose.pose.position.y, self.min_y, self.max_y) self.current_pose.pose.position.z = self.sanitize(self.current_pose.pose.position.z, self.min_z, self.max_z) self.current_pose.header.stamp = rospy.Time.now() if self.send_goals: # send MODIFIED GOALS self.pose_pub.publish(self.current_pose) else: self.last_pose_to_follow.header.stamp = rospy.Time.now() self.pose_pub.publish(self.last_pose_to_follow) self.debug_pose_pub.publish(self.current_pose) def get_force_movement(self): f_x = self.last_wrench.wrench.force.__getattribute__( self.frame_fixer.x_axis) f_y = self.last_wrench.wrench.force.__getattribute__( self.frame_fixer.y_axis) f_z = self.last_wrench.wrench.force.__getattribute__( self.frame_fixer.z_axis) return f_x, f_y, f_z def get_torque_movement(self): t_x = self.last_wrench.wrench.torque.__getattribute__( self.frame_fixer.roll_axis) t_y = self.last_wrench.wrench.torque.__getattribute__( self.frame_fixer.pitch_axis) t_z = self.last_wrench.wrench.torque.__getattribute__( self.frame_fixer.yaw_axis) return t_x, t_y, t_z def sanitize(self, data, min_v, max_v): if data > max_v: return max_v if data < min_v: return min_v return data
class ParticleFilter: """ The class that represents a Particle Filter ROS Node Attributes list: initialized: a Boolean flag to communicate to other class methods that initializaiton is complete base_frame: the name of the robot base coordinate frame (should be "base_link" for most robots) map_frame: the name of the map coordinate frame (should be "map" in most cases) odom_frame: the name of the odometry coordinate frame (should be "odom" in most cases) scan_topic: the name of the scan topic to listen to (should be "scan" in most cases) n_particles: the number of particles in the filter d_thresh: the amount of linear movement before triggering a filter update a_thresh: the amount of angular movement before triggering a filter update laser_max_distance: the maximum distance to an obstacle we should use in a likelihood calculation pose_listener: a subscriber that listens for new approximate pose estimates (i.e. generated through the rviz GUI) particle_pub: a publisher for the particle cloud laser_subscriber: listens for new scan data on topic self.scan_topic tf_listener: listener for coordinate transforms tf_broadcaster: broadcaster for coordinate transforms particle_cloud: a list of particles representing a probability distribution over robot poses current_odom_xy_theta: the pose of the robot in the odometry frame when the last filter update was performed. The pose is expressed as a list [x,y,theta] (where theta is the yaw) map: the map we will be localizing ourselves in. The map should be of type nav_msgs/OccupancyGrid """ def __init__(self): self.initialized = False # make sure we don't perform updates before everything is setup rospy.init_node('pf') # tell roscore that we are creating a new node named "pf" self.base_frame = "base_link" # the frame of the robot base self.map_frame = "map" # the name of the map coordinate frame self.odom_frame = "odom" # the name of the odometry coordinate frame self.scan_topic = "scan" # the topic where we will get laser scans from self.n_particles = 500 # the number of particles to use self.d_thresh = 0.2 # the amount of linear movement before performing an update self.a_thresh = math.pi/6 # the amount of angular movement before performing an update self.laser_max_distance = 2.0 # maximum penalty to assess in the likelihood field model self.sigma = 0.1 # TODO: define additional constants if needed # Setup pubs and subs # pose_listener responds to selection of a new approximate robot location (for instance using rviz) self.pose_listener = rospy.Subscriber("initialpose", PoseWithCovarianceStamped, self.update_initial_pose) # publish the current particle cloud. This enables viewing particles in rviz. self.particle_pub = rospy.Publisher("particlecloud", PoseArray, queue_size=10) # laser_subscriber listens for data from the lidar self.laser_subscriber = rospy.Subscriber(self.scan_topic, LaserScan, self.scan_received) # enable listening for and broadcasting coordinate transforms self.tf_listener = TransformListener() self.tf_broadcaster = TransformBroadcaster() self.particle_cloud = [] self.current_odom_xy_theta = [] rospy.wait_for_service("static_map") static_map = rospy.ServiceProxy("static_map", GetMap) try: map = static_map().map except: print("Could not receive map") # for now we have commented out the occupancy field initialization until you can successfully fetch the map self.occupancy_field = OccupancyField(map) self.initialized = True def update_robot_pose(self): """ Update the estimate of the robot's pose given the updated particles. There are two logical methods for this: (1): compute the mean pose (2): compute the most likely pose (i.e. the mode of the distribution) """ # first make sure that the particle weights are normalized self.normalize_particles() mean_x = 0 mean_y = 0 mean_theta = 0 mean_x_vector = 0 mean_y_vector = 0 for p in self.particle_cloud: mean_x += p.x*p.w mean_y += p.y*p.w mean_x_vector += math.cos(p.theta)*p.w mean_y_vector += math.sin(p.theta)*p.w mean_theta = math.atan2(mean_y_vector, mean_x_vector) self.robot_pose = Particle(x=mean_x,y=mean_y,theta=mean_theta).as_pose() def update_particles_with_odom(self, msg): """ Update the particles using the newly given odometry pose. The function computes the value delta which is a tuple (x,y,theta) that indicates the change in position and angle between the odometry when the particles were last updated and the current odometry. msg: this is not really needed to implement this, but is here just in case. """ new_odom_xy_theta = convert_pose_to_xy_and_theta(self.odom_pose.pose) # compute the change in x,y,theta since our last update if self.current_odom_xy_theta: old_odom_xy_theta = self.current_odom_xy_theta delta = {'x': new_odom_xy_theta[0] - self.current_odom_xy_theta[0], 'y': new_odom_xy_theta[1] - self.current_odom_xy_theta[1], 'theta': new_odom_xy_theta[2] - self.current_odom_xy_theta[2]} delta['r'] = math.sqrt(delta['x']**2 + delta['y']**2) delta['rot'] = angle_diff(math.atan2(delta['y'],delta['x']), old_odom_xy_theta[2]) self.current_odom_xy_theta = new_odom_xy_theta else: self.current_odom_xy_theta = new_odom_xy_theta return for p in self.particle_cloud: p.x += delta['r']*math.cos(delta['rot'] + p.theta) p.y += delta['r']*math.sin(delta['rot'] + p.theta) p.theta += delta['theta'] def map_calc_range(self,x,y,theta): """ Difficulty Level 3: implement a ray tracing likelihood model... Let me know if you are interested """ # TODO(NOPE): nothing unless you want to try this alternate likelihood model pass def resample_particles(self): """ Resample the particles according to the new particle weights. The weights stored with each particle should define the probability that a particular particle is selected in the resampling step. You may want to make use of the given helper function draw_random_sample. """ # make sure the distribution is normalized self.normalize_particles() indices = [i for i in range(len(self.particle_cloud))] probs = [p.w for p in self.particle_cloud] # print('b') # print(probs) new_indices = self.draw_random_sample(choices=indices, probabilities=probs, n=(self.n_particles)) new_particles = [] for i in new_indices: clean_index = int(i) old_particle = self.particle_cloud[clean_index] new_particles.append(Particle(x=old_particle.x+gauss(0,.05),y=old_particle.y+gauss(0,.05),theta=old_particle.theta+gauss(0,.05))) self.particle_cloud = new_particles self.normalize_particles() def update_particles_with_laser(self, msg): """ Updates the particle weights in response to the scan contained in the msg """ for p in self.particle_cloud: weight_sum = 0 for i in range(360): n_o = p.nearest_obstacle(i, msg.ranges[i]) error = self.occupancy_field.get_closest_obstacle_distance(n_o[0], n_o[1]) weight_sum += math.exp(-error*error/(2*self.sigma**2)) p.w = weight_sum / 360 # print(p.w) self.normalize_particles() @staticmethod def weighted_values(values, probabilities, size): """ Return a random sample of size elements from the set values with the specified probabilities values: the values to sample from (numpy.ndarray) probabilities: the probability of selecting each element in values (numpy.ndarray) size: the number of samples """ bins = np.add.accumulate(probabilities) return values[np.digitize(random_sample(size), bins)] @staticmethod def draw_random_sample(choices, probabilities, n): """ Return a random sample of n elements from the set choices with the specified probabilities choices: the values to sample from represented as a list probabilities: the probability of selecting each element in choices represented as a list n: the number of samples """ values = np.array(range(len(choices))) probs = np.array(probabilities) bins = np.add.accumulate(probs) inds = values[np.digitize(random_sample(n), bins)] samples = [] for i in inds: samples.append(deepcopy(choices[int(i)])) return samples def update_initial_pose(self, msg): """ Callback function to handle re-initializing the particle filter based on a pose estimate. These pose estimates could be generated by another ROS Node or could come from the rviz GUI """ xy_theta = convert_pose_to_xy_and_theta(msg.pose.pose) self.initialize_particle_cloud(xy_theta) self.fix_map_to_odom_transform(msg) def initialize_particle_cloud(self, xy_theta=None): """ Initialize the particle cloud. Arguments xy_theta: a triple consisting of the mean x, y, and theta (yaw) to initialize the particle cloud around. If this input is ommitted, the odometry will be used """ if xy_theta == None: xy_theta = convert_pose_to_xy_and_theta(self.odom_pose.pose) self.particle_cloud = [] for i in range(self.n_particles): self.particle_cloud.append(Particle(x=xy_theta[0]+gauss(0,0.25),y=xy_theta[1]+gauss(0,0.25),theta=xy_theta[2]+gauss(0,0.25))) self.normalize_particles() self.update_robot_pose() def normalize_particles(self): """ Make sure the particle weights define a valid distribution (i.e. sum to 1.0) """ w = [deepcopy(p.w) for p in self.particle_cloud] z = sum(w) print(z) if z > 0: for i in range(len(self.particle_cloud)): self.particle_cloud[i].w = w[i] / z else: for i in range(len(self.particle_cloud)): self.particle_cloud[i].w = 1/len(self.particle_cloud) print(sum([p.w for p in self.particle_cloud])) def publish_particles(self, msg): particles_conv = [] for p in self.particle_cloud: particles_conv.append(p.as_pose()) # actually send the message so that we can view it in rviz self.particle_pub.publish(PoseArray(header=Header(stamp=rospy.Time.now(), frame_id=self.map_frame), poses=particles_conv)) def scan_received(self, msg): """ This is the default logic for what to do when processing scan data. Feel free to modify this, however, I hope it will provide a good guide. The input msg is an object of type sensor_msgs/LaserScan """ if not(self.initialized): # wait for initialization to complete return if not(self.tf_listener.canTransform(self.base_frame,msg.header.frame_id,msg.header.stamp)): # need to know how to transform the laser to the base frame # this will be given by either Gazebo or neato_node return if not(self.tf_listener.canTransform(self.base_frame,self.odom_frame,msg.header.stamp)): # need to know how to transform between base and odometric frames # this will eventually be published by either Gazebo or neato_node return # calculate pose of laser relative ot the robot base p = PoseStamped(header=Header(stamp=rospy.Time(0), frame_id=msg.header.frame_id)) self.laser_pose = self.tf_listener.transformPose(self.base_frame,p) # find out where the robot thinks it is based on its odometry p = PoseStamped(header=Header(stamp=msg.header.stamp, frame_id=self.base_frame), pose=Pose()) self.odom_pose = self.tf_listener.transformPose(self.odom_frame, p) # store the the odometry pose in a more convenient format (x,y,theta) new_odom_xy_theta = convert_pose_to_xy_and_theta(self.odom_pose.pose) if not(self.particle_cloud): # now that we have all of the necessary transforms we can update the particle cloud self.initialize_particle_cloud() # cache the last odometric pose so we can only update our particle filter if we move more than self.d_thresh or self.a_thresh self.current_odom_xy_theta = new_odom_xy_theta # update our map to odom transform now that the particles are initialized self.fix_map_to_odom_transform(msg) elif (math.fabs(new_odom_xy_theta[0] - self.current_odom_xy_theta[0]) > self.d_thresh or math.fabs(new_odom_xy_theta[1] - self.current_odom_xy_theta[1]) > self.d_thresh or math.fabs(new_odom_xy_theta[2] - self.current_odom_xy_theta[2]) > self.a_thresh): # we have moved far enough to do an update! self.update_particles_with_odom(msg) # update based on odometry self.update_particles_with_laser(msg) # update based on laser scan self.update_robot_pose() # update robot's pose self.resample_particles() # resample particles to focus on areas of high density self.fix_map_to_odom_transform(msg) # update map to odom transform now that we have new particles # publish particles (so things like rviz can see them) self.publish_particles(msg) def fix_map_to_odom_transform(self, msg): """ This method constantly updates the offset of the map and odometry coordinate systems based on the latest results from the localizer """ (translation, rotation) = convert_pose_inverse_transform(self.robot_pose) p = PoseStamped(pose=convert_translation_rotation_to_pose(translation,rotation), header=Header(stamp=msg.header.stamp,frame_id=self.base_frame)) self.odom_to_map = self.tf_listener.transformPose(self.odom_frame, p) (self.translation, self.rotation) = convert_pose_inverse_transform(self.odom_to_map.pose) def broadcast_last_transform(self): """ Make sure that we are always broadcasting the last map to odom transformation. This is necessary so things like move_base can work properly. """ if not(hasattr(self,'translation') and hasattr(self,'rotation')): return self.tf_broadcaster.sendTransform(self.translation, self.rotation, rospy.get_rostime(), self.odom_frame, self.map_frame)
class ContourFollower(): def __init__(self, group): rospy.init_node('contour_follower', anonymous=True) rospy.Subscriber('contour_path', Path, self.path_cb) self.traj_pub = rospy.Publisher('/corrected_path', Path, queue_size=10) self.header = None self.robot = moveit_commander.RobotCommander() self.scene = moveit_commander.PlanningSceneInterface() self.group = moveit_commander.MoveGroupCommander(group) self.listener = TransformListener() self.vertical_offset = rospy.get_param('countour_offset', 0.3) self.repositioning_offset = rospy.get_param('repositioning_offset', 0.3) rospy.spin() def path_cb(self, msg): planning_frame = self.group.get_planning_frame() self.listener.waitForTransform(msg.header.frame_id, planning_frame, rospy.Time.now(), rospy.Duration(1.0)) if True: #self.listener.frameExists(msg.header.frame_id) and self.listener.frameExists('/camera_color/optical_frame'): rospy.loginfo('Planning path through waypoints') pose = self.group.get_current_pose().pose pose_start = copy.deepcopy(pose) pose.position = self.listener.transformPose( planning_frame, msg.poses[0]).pose.position pose.position.z = pose.position.z - self.repositioning_offset self.group.set_pose_target(pose) plan = self.group.go(wait=True) if not plan: return self.group.stop() print('Moved to start point') waypoints = [copy.deepcopy(self.group.get_current_pose().pose)] path = Path() path.header.frame_id = planning_frame p_pose = geometry_msgs.msg.PoseStamped() for j in range(len(msg.poses)): print(waypoints[-1]) pose = waypoints[-1] pose.position = self.listener.transformPose( planning_frame, msg.poses[j]).pose.position pose.position.z = pose.position.z - self.vertical_offset waypoints.append(copy.deepcopy(pose)) p_pose.pose = pose path.poses.append(copy.deepcopy(p_pose)) self.traj_pub.publish(path) print(waypoints[-1]) plan, fraction = self.group.compute_cartesian_path( waypoints[1:], 0.2, 0.0) for j in range(len(plan.joint_trajectory.points)): plan.joint_trajectory.points[ j].time_from_start = rospy.Duration(j) rospy.loginfo('Planning done') self.group.execute(plan, wait=True) p_pose = path.poses[-1] p_pose.pose.position.z = p_pose.pose.position.z self.group.set_pose_target(p_pose.pose) plan = self.group.go(wait=True) if not plan: return self.group.stop() print('Moved above end') self.group.set_pose_target(pose_start) plan = self.group.go(wait=True) if not plan: return self.group.stop() print('Moved to start point') else: rospy.logerr('No transform between ' + planning_frame + ' and ' + msg.header.frame_id)
class ArmIntermediary(): def __init__(self, arm): self.arm = arm self.tfl = TransformListener() self.pr2_arm = PR2Arm(self.arm, self.tfl) self.pr2_gripper = PR2Gripper(self.arm) rospy.loginfo('Waiting for Pixel_2_3d Service') try: rospy.wait_for_service('/pixel_2_3d', 7.0) self.p23d_proxy = rospy.ServiceProxy('/pixel_2_3d', Pixel23d, True) rospy.loginfo("Found pixel_2_3d service") except: rospy.logwarn("Pixel_2_3d Service Not available") #Low-level motion requests: pass directly to pr2_arm rospy.Subscriber("wt_"+self.arm+"_arm_pose_commands", Point, self.torso_frame_move) rospy.Subscriber("wt_"+self.arm+"_arm_angle_commands", JointTrajectoryPoint, self.pr2_arm.send_traj_point) #More complex motion scripts, built here using pr2_arm functions rospy.Subscriber("norm_approach_"+self.arm, PoseStamped, self.norm_approach) rospy.Subscriber("wt_grasp_"+self.arm+"_goal", PoseStamped, self.grasp) rospy.Subscriber("wt_wipe_"+self.arm+"_goals", PoseStamped, self.wipe) rospy.Subscriber("wt_swipe_"+self.arm+"_goals", PoseStamped, self.swipe) rospy.Subscriber("wt_lin_move_"+self.arm, Float32, self.hand_move) rospy.Subscriber("wt_adjust_elbow_"+self.arm, Float32, self.pr2_arm.adjust_elbow) rospy.Subscriber('wt_surf_wipe_right_points', Point, self.prep_surf_wipe) rospy.Subscriber("wt_poke_"+self.arm+"_point", PoseStamped, self.poke) rospy.Subscriber(rospy.get_name()+"/log_out", String, self.repub_log) rospy.Subscriber("wt_"+self.arm[0]+"_gripper_commands", Pr2GripperCommand, self.gripper_pos) rospy.Subscriber("wt_"+self.arm[0]+"_gripper_grab_commands", Bool, self.gripper_grab) rospy.Subscriber("wt_"+self.arm[0]+"_gripper_release_commands", Bool, self.gripper_release) self.wt_log_out = rospy.Publisher("wt_log_out", String) self.wipe_started = False self.surf_wipe_started = False self.wipe_ends = [PoseStamped(), PoseStamped()] def repub_log(self, msg): self.wt_log_out.publish(msg) def gripper_pos(self, msg): self.pr2_gripper.gripper_action(msg.position, msg.max_effort) def gripper_grab(self, msg): self.pr2_gripper.grab() def gripper_release(self, msg): self.pr2_gripper.release() def torso_frame_move(self, msg): """Do linear motion relative to torso frame.""" goal = self.pr2_arm.curr_pose() goal.pose.position.x += msg.x goal.pose.position.y += msg.y goal.pose.position.z += msg.z self.pr2_arm.blind_move(goal) def hand_move(self, f32): """Do linear motion relative to hand frame.""" hfm_pose = self.pr2_arm.hand_frame_move(f32.data) self.pr2_arm.blind_move(hfm_pose) def norm_approach(self, pose): """ Safe move normal to surface pose at given distance.""" appr = pu.find_approach(pose, 0.25) self.pr2_arm.move_arm_to(appr) def grasp(self, ps): """Do simple grasp: Normal approch, open, advance, close, retreat.""" rospy.loginfo("Initiating Grasp Sequence") self.wt_log_out.publish(data="Initiating Grasp Sequence") approach = pose_utils.find_approach(ps, 0.15) rospy.loginfo("approach: \r\n %s" %approach) at_appr = self.pr2_arm.move_arm_to(approach) rospy.loginfo("arrived at approach: %s" %at_appr) if at_appr: opened = self.pr2_arm.gripper(0.09) if opened: rospy.loginfo("making linear approach") hfm_pose = pose_utils.find_approach(ps, -0.02) self.pr2_arm.blind_move(hfm_pose) self.pr2_arm.wait_for_stop() closed = self.pr2_arm.gripper(0.0) if not closed: rospy.loginfo("Couldn't close completely:\ Grasp likely successful") hfm_pose = self.pr2_arm.hand_frame_move(-0.20) self.pr2_arm.blind_move(hfm_pose) else: pass def prep_surf_wipe(self, point): pixel_u = point.x pixel_v = point.y test_pose = self.p23d_proxy(pixel_u, pixel_v).pixel3d test_pose = pose_utils.find_approach(test_pose, 0) (reachable, ik_goal) = self.pr2_arm.full_ik_check(test_pose) if reachable: if not self.surf_wipe_started: start_pose = test_pose self.surf_wipe_start = [pixel_u, pixel_v, start_pose] self.surf_wipe_started = True rospy.loginfo("Received valid starting position for wiping\ action") self.wt_log_out.publish(data="Received valid starting position\ for wiping action") return #Return after 1st point, wait for second else: rospy.loginfo("Received valid ending position for wiping\ action") self.wt_log_out.publish(data="Received valid ending position\ for wiping action") self.surf_wipe_started = False else: rospy.loginfo("Cannot reach wipe position, please try another") self.wt_log_out.publish(data="Cannot reach wipe position,\ please try another") return #Return on invalid point, wait for another dist = self.pr2_arm.calc_dist(self.surf_wipe_start[2],test_pose) print 'dist', dist num_points = dist/0.02 print 'num_points', num_points us = np.round(np.linspace(self.surf_wipe_start[0], pixel_u, num_points)) vs = np.round(np.linspace(self.surf_wipe_start[1], pixel_v, num_points)) surf_points = [PoseStamped() for i in xrange(len(us))] print "Surface Points", [us,vs] for i in xrange(len(us)): pose = self.p23d_proxy(us[i],vs[i]).pixel3d surf_points[i] = pose_utils.find_approach(pose,0) print i+1, '/', len(us) self.pr2_arm.blind_move(surf_points[0]) self.pr2_arm.wait_for_stop() for pose in surf_points: self.pr2_arm.blind_move(pose,2.5) rospy.sleep(2) self.pr2_arm.hand_frame_move(-0.1) def poke(self, ps): appr = pose_utils.find_approach(ps,0.15) prepared = self.pr2_arm.move_arm_to(appr) if prepared: pt1 = self.pr2_arm.hand_frame_move(0.155) self.pr2_arm.blind_move(pt1) self.pr2_arm.wait_for_stop() pt2 = self.pr2_arm.hand_frame_move(-0.155) self.pr2_arm.blind_move(pt2) def swipe(self, ps): traj = self.prep_wipe(ps) if traj is not None: self.wipe_move(traj, 1) def wipe(self, ps): traj = self.prep_wipe(ps) if traj is not None: self.wipe_move(traj, 4) def prep_wipe(self, ps): #print "Prep Wipe Received: %s" %pa print "Updating frame to: %s \r\n" %ps if not self.wipe_started: self.wipe_appr_seed = ps self.wipe_ends[0] = pose_utils.find_approach(ps, 0) print "wipe_end[0]: %s" %self.wipe_ends[0] (reachable, ik_goal) = self.pr2_arm.full_ik_check(self.wipe_ends[0]) if not reachable: rospy.loginfo("Cannot find approach for initial wipe position,\ please try another") self.wt_log_out.publish(data="Cannot find approach for initial\ wipe position, please try another") return None else: self.wipe_started = True rospy.loginfo("Received starting position for wiping action") self.wt_log_out.publish(data="Received starting position for\ wiping action") return None else: self.wipe_ends[1] = pose_utils.find_approach(ps, 0) self.wipe_ends.reverse() (reachable, ik_goal) = self.pr2_arm.full_ik_check(self.wipe_ends[1]) if not reachable: rospy.loginfo("Cannot find approach for final wipe position,\ please try another") self.wt_log_out.publish(data="Cannot find approach for final\ wipe position, please try another") return None else: rospy.loginfo("Received End position for wiping action") self.wt_log_out.publish(data="Received End position for wiping\ action") self.wipe_ends[1].header.stamp = rospy.Time.now() self.tfl.waitForTransform(self.wipe_ends[1].header.frame_id, 'rh_utility_frame', rospy.Time.now(), rospy.Duration(3.0)) fin_in_start = self.tfl.transformPose('rh_utility_frame', self.wipe_ends[1]) ang = math.atan2(-fin_in_start.pose.position.z, -fin_in_start.pose.position.y)+(math.pi/2) q_st_rot = transformations.quaternion_about_axis(ang, (1,0,0)) q_st_new = transformations.quaternion_multiply( [self.wipe_ends[0].pose.orientation.x, self.wipe_ends[0].pose.orientation.y, self.wipe_ends[0].pose.orientation.z, self.wipe_ends[0].pose.orientation.w], q_st_rot) self.wipe_ends[0].pose.orientation = Quaternion(*q_st_new) self.wipe_ends[0].header.stamp = rospy.Time.now() self.tfl.waitForTransform(self.wipe_ends[0].header.frame_id, 'rh_utility_frame', rospy.Time.now(), rospy.Duration(3.0)) start_in_fin = self.tfl.transformPose('rh_utility_frame', self.wipe_ends[0]) ang = math.atan2(start_in_fin.pose.position.z, start_in_fin.pose.position.y)+(math.pi/2) q_st_rot = transformations.quaternion_about_axis(ang, (1,0,0)) q_st_new = transformations.quaternion_multiply( [self.wipe_ends[1].pose.orientation.x, self.wipe_ends[1].pose.orientation.y, self.wipe_ends[1].pose.orientation.z, self.wipe_ends[1].pose.orientation.w], q_st_rot) self.wipe_ends[1].pose.orientation = Quaternion(*q_st_new) appr = pose_utils.find_approach(self.wipe_appr_seed, 0.15) appr.pose.orientation = self.wipe_ends[1].pose.orientation prepared = self.pr2_arm.move_arm_to(appr) if prepared: self.pr2_arm.blind_move(self.wipe_ends[1]) traj = self.pr2_arm.build_trajectory(self.wipe_ends[1], self.wipe_ends[0]) wipe_traj = self.pr2_arm.build_follow_trajectory(traj) self.pr2_arm.wait_for_stop() self.wipe_started = False return wipe_traj #self.wipe(wipe_traj) else: rospy.loginfo("Failure reaching start point,\ please try again") self.wt_log_out.publish(data="Failure reaching start\ point, please try again") def wipe_move(self, traj_goal, passes=4): times = [] for i in range(len(traj_goal.trajectory.points)): times.append(traj_goal.trajectory.points[i].time_from_start) count = 0 while count < passes: traj_goal.trajectory.points.reverse() for i in range(len(times)): traj_goal.trajectory.points[i].time_from_start = times[i] traj_goal.trajectory.header.stamp = rospy.Time.now() assert traj_goal.trajectory.points[0] != [] self.pr2_arm.r_arm_follow_traj_client.send_goal(traj_goal) self.pr2_arm.r_arm_follow_traj_client.wait_for_result( rospy.Duration(20)) rospy.sleep(0.5)# Pause at end of swipe count += 1 rospy.loginfo("Done Wiping") self.wt_log_out.publish(data="Done Wiping") hfm_pose = self.pr2_arm.hand_frame_move(-0.15) self.pr2_arm.blind_move(hfm_pose)
class PR2Greeter: def __init__(self, debug=False, online = True, robot_frame="odom_combined"): self._tf = TransformListener() self._online = online # self.snd_handle = SoundClient() if self._online: #self._interface = ROSpeexInterface() #self._interface.init() self._speech_client = SpeechSynthesisClient_NICT() else: self.snd_handle = SoundClient() rospy.sleep(1) self.say('Hello world!') rospy.sleep(1) self._debug = debug self._robot_frame = robot_frame self._point_sub = rospy.Subscriber('nearest_face', PointStamped, self.face_cb) self._head_action_cl = actionlib.SimpleActionClient('/head_traj_controller/point_head_action', pr2_controllers_msgs.msg.PointHeadAction) self._torso_action_cl = actionlib.SimpleActionClient('/torso_controller/position_joint_action', pr2_controllers_msgs.msg.SingleJointPositionAction) self._left_arm = MoveGroupCommander("left_arm") self._right_arm = MoveGroupCommander("right_arm") print "r.f.: " + self._left_arm.get_pose_reference_frame() self.face = None # self.face_from = rospy.Time(0) self.face_last_dist = 0 self.last_invited_at = rospy.Time(0) self._person_prob_left = 0 self.l_home_pose = [0.283, 0.295, 0.537, -1.646, 0.468, -1.735] self.l_wave_1 = [-0.1, 0.6, 1.15, -1.7, -0.97, -1.6] self.l_wave_2 = [-0.1, 0.6, 1.15, 1.7, -0.97, 1.6] self.r_home_pose = [0.124, -0.481, 0.439, -1.548, 0.36, -0.035] self.r_advert = [0.521, -0.508, 0.845, -1.548, 0.36, -0.035] self.no_face_random_delay = None self._initialized = False self._timer = rospy.Timer(rospy.Duration(1.0), self.timer) self._move_buff = Queue.Queue() self._head_buff = Queue.Queue() self._move_thread = threading.Thread(target=self.movements) self._move_thread.daemon = True self._move_thread.start() self._head_thread = threading.Thread(target=self.head) self._head_thread.daemon = True self._head_thread.start() self.new_face = False self.face_last_dist = 0.0 self.face_counter = 0 self.actions = [self.stretchingAction, self.introduceAction, self.waveHandAction, self.lookAroundAction, self.lookAroundAction, self.lookAroundAction, self.advertAction, self.numberOfFacesAction] self.goodbye_strings = ["Thanks for stopping by.", "Enjoy the event.", "See you later!", "Have a nice day!"] self.invite_strings = ["Hello. It's nice to see you.", "Come here and take some flyer.", "I hope you are enjoying the event."] rospy.loginfo("Ready") def say(self, text): if self._online: #self._interface.say(text, 'en', 'nict') data = self._speech_client.request(text, 'en') try: tmp_file = open('output_tmp.dat', 'wb') tmp_file.write(data) tmp_file.close() # play sound subprocess.check_output(['ffplay', 'output_tmp.dat', '-autoexit', '-nodisp'], stderr=subprocess.STDOUT) except IOError: rospy.logerr('file io error.') except OSError: rospy.logerr('ffplay is not installed.') except subprocess.CalledProcessError: rospy.logerr('ffplay return error value.') except: rospy.logerr('unknown exception.') else: self.snd_handle.say(text) def getRandomFromArray(self, arr): idx = random.randint(0, len(arr) - 1) return arr[idx] def stretchingAction(self): self.say("I'm bit tired. Let's do some stretching.") self._torso_action_cl.wait_for_server() goal = pr2_controllers_msgs.msg.SingleJointPositionGoal() goal.position = 0.195 goal.max_velocity = 0.5 try: self._torso_action_cl.send_goal(goal) self._torso_action_cl.wait_for_result(rospy.Duration.from_sec(10.0)) except: rospy.logerr("torso action error (up)") # TODO move arms self.say("I feel much better now!") goal.position = 0.0 try: self._torso_action_cl.send_goal(goal) self._torso_action_cl.wait_for_result(rospy.Duration.from_sec(10.0)) except: rospy.logerr("torso action error (down)") def numberOfFacesAction(self): string = "Today I already saw " + str(self.face_counter) + "face" if self.face_counter != 1: string = string + "s" string = string + "." self.say(string) rospy.sleep(1) def advertAction(self): self.say("Hello. Here are some posters for you.") self.go(self._right_arm, self.r_advert) rospy.sleep(1) def introduceAction(self): self.say("Hello. I'm PR2 robot. Come here to check me.") rospy.sleep(1) def waveHandAction(self): self.say("I'm here. Please come to see me.") rand = random.randint(1, 3) for _ in range(rand): self.wave() self.go(self._left_arm, self.l_home_pose) rospy.loginfo("Waving") rospy.sleep(1) def lookAroundAction(self): self.say("I'm looking for somebody. Please come closer.") p = PointStamped() p.header.stamp = rospy.Time.now() p.header.frame_id = "/base_link" p.point.x = 5.0 sign = random.choice([-1, 1]) p.point.y = sign * random.uniform(1.5, 0.5) p.point.z = random.uniform(1.7, 0.2) self._head_buff.put((copy.deepcopy(p), 0.1, True)) p.point.y = -1 * sign * random.uniform(1.5, 0.5) p.point.z = random.uniform(1.7, 0.2) self._head_buff.put((copy.deepcopy(p), 0.1, True)) p.point.y = sign * random.uniform(1.5, 0.5) p.point.z = random.uniform(1.7, 0.2) self._head_buff.put((copy.deepcopy(p), 0.1, True)) p.point.y = -1 * sign * random.uniform(1.5, 0.5) p.point.z = random.uniform(1.7, 0.2) self._head_buff.put((copy.deepcopy(p), 0.1, True)) rospy.loginfo("Looking around") rospy.sleep(1) def getPointDist(self, pt): assert(self.face is not None) # fist, get my position p = PoseStamped() p.header.frame_id = "base_link" p.header.stamp = rospy.Time.now() - rospy.Duration(0.5) p.pose.position.x = 0 p.pose.position.y = 0 p.pose.position.z = 0 p.pose.orientation.x = 0 p.pose.orientation.y = 0 p.pose.orientation.z = 0 p.pose.orientation.w = 1 try: self._tf.waitForTransform(p.header.frame_id, self._robot_frame, p.header.stamp, rospy.Duration(2)) p = self._tf.transformPose(self._robot_frame, p) except: rospy.logerr("TF error!") return None return sqrt(pow(p.pose.position.x - pt.point.x, 2) + pow(p.pose.position.y - pt.point.y, 2) + pow(p.pose.position.z - pt.point.z, 2)) def getPoseStamped(self, group, c): assert(len(c) == 6) p = PoseStamped() p.header.frame_id = "base_link" p.header.stamp = rospy.Time.now() - rospy.Duration(0.5) p.pose.position.x = c[0] p.pose.position.y = c[1] p.pose.position.z = c[2] quat = tf.transformations.quaternion_from_euler(c[3], c[4], c[5]) p.pose.orientation.x = quat[0] p.pose.orientation.y = quat[1] p.pose.orientation.z = quat[2] p.pose.orientation.w = quat[3] try: self._tf.waitForTransform(p.header.frame_id, group.get_pose_reference_frame(), p.header.stamp, rospy.Duration(2)) p = self._tf.transformPose(group.get_pose_reference_frame(), p) except: rospy.logerr("TF error!") return None return p def go(self, group, where): self._move_buff.put((group, where)) def wave(self): self.go(self._left_arm, self.l_wave_1) self.go(self._left_arm, self.l_wave_2) self.go(self._left_arm, self.l_wave_1) def head(self): self._head_action_cl.wait_for_server() while not rospy.is_shutdown(): (target, vel, imp) = self._head_buff.get() # we don't need to block it here self._head_buff.task_done() if (not imp) and (not self._head_buff.empty()): print "skipping head goal" # head target can be skipped (follow only the latest one) continue # print "head point goal" # print target # point PR2's head there (http://wiki.ros.org/pr2_controllers/Tutorials/Moving%20the%20Head) goal = pr2_controllers_msgs.msg.PointHeadGoal() goal.target = target # goal.min_duration = rospy.Duration(3.0) goal.max_velocity = vel # goal.pointing_frame = "high_def_frame" goal.pointing_frame = "head_mount_kinect_rgb_link" goal.pointing_axis.x = 1 goal.pointing_axis.y = 0 goal.pointing_axis.z = 0 try: self._head_action_cl.send_goal(goal) self._head_action_cl.wait_for_result(rospy.Duration.from_sec(5.0)) except: rospy.logerr("head action error") #self._head_buff.task_done() def movements(self): while not rospy.is_shutdown(): (group, where) = self._move_buff.get() group.set_start_state_to_current_state() p = self.getPoseStamped(group, where) if p is None: self._move_buff.task_done() continue group.set_pose_target(p) group.set_goal_tolerance(0.1) group.allow_replanning(True) self._move_buff.task_done() group.go(wait=True) def timer(self, event): if self._initialized is False: rospy.loginfo("Moving arms to home positions") self.init_head() self.go(self._left_arm, self.l_home_pose) self.go(self._right_arm, self.r_home_pose) self._move_buff.join() self.say("I'm ready for a great job.") self._initialized = True if self.face is None: if (self.no_face_random_delay is None): delay = random.uniform(30, 10) self.no_face_random_delay = rospy.Time.now() + rospy.Duration(delay) rospy.loginfo("Random delay: " + str(delay)) return else: if rospy.Time.now() < self.no_face_random_delay: return self.init_head() self.go(self._left_arm, self.l_home_pose) self.go(self._right_arm, self.r_home_pose) rospy.loginfo("Starting selected action") action = self.getRandomFromArray(self.actions) action() delay = random.uniform(30, 10) self.no_face_random_delay = rospy.Time.now() + rospy.Duration(delay) return else: self.no_face_random_delay = None if self.new_face and (self.last_invited_at + rospy.Duration(10) < rospy.Time.now()): self.last_invited_at = rospy.Time.now() self.new_face = False rospy.loginfo("new person") self.face_counter = self.face_counter + 1 # cd = getPointDist(self.face) # TODO decide action based on distance ? self.go(self._left_arm, self.l_home_pose) # if a person is too close, dont move hand? self.go(self._right_arm, self.r_advert) string = self.getRandomFromArray(self.invite_strings) self.say(string) # TODO wait some min. time + say something # after 20 seconds of no detected face, let's continue if self.face.header.stamp + rospy.Duration(10) < rospy.Time.now(): rospy.loginfo("person left") string = self.getRandomFromArray(self.goodbye_strings) self.say(string) self.init_head() self.go(self._left_arm, self.l_home_pose) self.go(self._right_arm, self.r_home_pose) self.face = None return self._head_buff.put((copy.deepcopy(self.face), 0.4, False)) def init_head(self): p = PointStamped() p.header.stamp = rospy.Time.now() p.header.frame_id = "/base_link" p.point.x = 2.0 p.point.y = 0.0 p.point.z = 1.7 self._head_buff.put((p, 0.1, True)) def face_cb(self, point): # transform point try: self._tf.waitForTransform(point.header.frame_id, self._robot_frame, point.header.stamp, rospy.Duration(2)) pt = self._tf.transformPoint(self._robot_frame, point) except: rospy.logerr("Transform error") return if self.face is not None: cd = self.getPointDist(pt) # current distance dd = fabs(self.face_last_dist - cd) # change in distance if dd < 0.5: self.face.header = pt.header # filter x,y,z values a bit self.face.point = pt.point #self.face.point.x = (self.face.point.x + pt.point.x) / 2 #self.face.point.y = (self.face.point.y + pt.point.y) / 2 #self.face.point.z = (self.face.point.z + pt.point.z) / 2 else: if self._person_prob_left < 2: self._person_prob_left += 1 else: self._person_prob_left = 0 print "new face ddist: " + str(dd) self.new_face = True self.face = pt self.face_last_dist = cd else: self._person_prob_left = 0 self.new_face = True self.face = pt
class ParticleFilter: def __init__(self): # once everything is setup initialized will be set to true self.initialized = False # initialize this particle filter node rospy.init_node('turtlebot3_particle_filter') # set the topic names and frame names self.base_frame = "base_footprint" self.map_topic = "map" self.odom_frame = "odom" self.scan_topic = "scan" # inialize our map self.map = OccupancyGrid() # the number of particles used in the particle filter self.num_particles = 10000 # initialize the particle cloud array self.particle_cloud = [] # initialize the estimated robot pose self.robot_estimate = Pose() # set threshold values for linear and angular movement before we preform an update self.lin_mvmt_threshold = 0.2 self.ang_mvmt_threshold = (np.pi / 6) self.odom_pose_last_motion_update = None self.likelihood_field = LikelihoodField() # Setup publishers and subscribers # publish the current particle cloud self.particles_pub = rospy.Publisher("particle_cloud", PoseArray, queue_size=10) # publish the estimated robot pose self.robot_estimate_pub = rospy.Publisher("estimated_robot_pose", PoseStamped, queue_size=10) # subscribe to the map server rospy.Subscriber(self.map_topic, OccupancyGrid, self.get_map) # subscribe to the lidar scan from the robot rospy.Subscriber(self.scan_topic, LaserScan, self.robot_scan_received) # enable listening for and broadcasting corodinate transforms self.tf_listener = TransformListener() self.tf_broadcaster = TransformBroadcaster() time.sleep(2) # intialize the particle cloud self.initialize_particle_cloud() self.initialized = True def get_map(self, data): print(data.info) self.map = data def in_building(self, x, y): # helper function that confirms whether a given x, y are within the house grid_x = math.floor(x) grid_y = math.floor(y) grid = self.map.data index = grid_y * self.map.info.width + grid_x if index < len(grid) and grid[index] == 0: return True return False def initialize_particle_cloud(self): # this generates a bunch of particles to fill the particle cloud while len(self.particle_cloud) < self.num_particles: # randomly choose x and y based on height and width of map and confirm they are in the house map_x = random_sample() * self.map.info.width map_y = random_sample() * self.map.info.height if not self.in_building(map_x, map_y): continue map_x = map_x - self.map.info.width * 0.5 - 10 map_y = map_y - self.map.info.height * 0.5 - 10 x = map_x * self.map.info.resolution y = map_y * self.map.info.resolution z = random_sample() * 360 z = np.deg2rad(z) # finalize x, y, and z(yaw) value # create new pose p = Pose() p.position.x = x p.position.y = y p.position.z = 0 q = quaternion_from_euler(0.0, 0.0, z) p.orientation.x = q[0] p.orientation.y = q[1] p.orientation.z = q[2] p.orientation.w = q[3] new_particle = Particle(p, 1.0) self.particle_cloud.append(new_particle) # normalize particle weights then make particle cloud accessible self.normalize_particles() self.publish_particle_cloud() def normalize_particles(self): # make all the particle weights sum to 1.0 # calculate total weight total_weight = 0 for particle in self.particle_cloud: total_weight = total_weight + particle.w # recalculate particles weight new_tot = 0 for particle in self.particle_cloud: particle.w = particle.w / total_weight new_tot += particle.w def publish_particle_cloud(self): particle_cloud_pose_array = PoseArray() particle_cloud_pose_array.header = Header(stamp=rospy.Time.now(), frame_id=self.map_topic) particle_cloud_pose_array.poses for part in self.particle_cloud: particle_cloud_pose_array.poses.append(part.pose) self.particles_pub.publish(particle_cloud_pose_array) def publish_estimated_robot_pose(self): robot_pose_estimate_stamped = PoseStamped() robot_pose_estimate_stamped.pose = self.robot_estimate robot_pose_estimate_stamped.header = Header(stamp=rospy.Time.now(), frame_id=self.map_topic) self.robot_estimate_pub.publish(robot_pose_estimate_stamped) def resample_particles(self): # new_cloud = [] # create new probs list which is the weight of each particle probs = list(map(lambda p: p.w, self.particle_cloud)) # new particle cloud calculated with the random.choice function new_cloud = np.random.choice(self.particle_cloud, size=self.num_particles, p=probs) # moves new particles from new_cloud to the particle cloud for i in range(self.num_particles): p = self.particle_cloud[i] new_p = new_cloud[i] p.pose.position.x = new_p.pose.position.x p.pose.position.y = new_p.pose.position.y p.pose.orientation.x = new_p.pose.orientation.x p.pose.orientation.y = new_p.pose.orientation.y p.pose.orientation.z = new_p.pose.orientation.z p.pose.orientation.w = new_p.pose.orientation.w return def robot_scan_received(self, data): # wait until initialization is complete if not (self.initialized): return # we need to be able to transfrom the laser frame to the base frame if not (self.tf_listener.canTransform( self.base_frame, data.header.frame_id, data.header.stamp)): return # wait for a little bit for the transform to become avaliable (in case the scan arrives # a little bit before the odom to base_footprint transform was updated) self.tf_listener.waitForTransform(self.base_frame, self.odom_frame, data.header.stamp, rospy.Duration(0.5)) if not (self.tf_listener.canTransform( self.base_frame, data.header.frame_id, data.header.stamp)): return # calculate the pose of the laser distance sensor p = PoseStamped( header=Header(stamp=rospy.Time(0), frame_id=data.header.frame_id)) self.laser_pose = self.tf_listener.transformPose(self.base_frame, p) # determine where the robot thinks it is based on its odometry p = PoseStamped(header=Header(stamp=data.header.stamp, frame_id=self.base_frame), pose=Pose()) self.odom_pose = self.tf_listener.transformPose(self.odom_frame, p) # we need to be able to compare the current odom pose to the prior odom pose # if there isn't a prior odom pose, set the odom_pose variable to the current pose if not self.odom_pose_last_motion_update: self.odom_pose_last_motion_update = self.odom_pose return if self.particle_cloud: # check to see if we've moved far enough to perform an update curr_x = self.odom_pose.pose.position.x old_x = self.odom_pose_last_motion_update.pose.position.x curr_y = self.odom_pose.pose.position.y old_y = self.odom_pose_last_motion_update.pose.position.y curr_yaw = get_yaw_from_pose(self.odom_pose.pose) old_yaw = get_yaw_from_pose(self.odom_pose_last_motion_update.pose) if (np.abs(curr_x - old_x) > self.lin_mvmt_threshold or np.abs(curr_y - old_y) > self.lin_mvmt_threshold or np.abs(curr_yaw - old_yaw) > self.ang_mvmt_threshold): # This is where the main logic of the particle filter is carried out self.update_particles_with_motion_model() self.update_particle_weights_with_measurement_model(data) self.normalize_particles() self.resample_particles() self.update_estimated_robot_pose() self.publish_particle_cloud() self.publish_estimated_robot_pose() self.odom_pose_last_motion_update = self.odom_pose def update_estimated_robot_pose(self): # based on the particles within the particle cloud, update the robot pose estimate cur_x = 0 cur_y = 0 cur_yaw = 0 # sum up the x, y, and yaw values of all particles for particle in self.particle_cloud: yaw = get_yaw_from_pose(particle.pose) cur_x = cur_x + particle.pose.position.x cur_y = cur_y + particle.pose.position.y cur_yaw = cur_yaw + yaw # get averages by dividing by number of particles cur_x = cur_x / self.num_particles cur_y = cur_y / self.num_particles cur_yaw = cur_yaw / self.num_particles # update robot estimate to equal the new pose p = self.robot_estimate p.position.x = cur_x p.position.y = cur_y q = quaternion_from_euler(0, 0, cur_yaw) p.orientation.x = q[0] p.orientation.y = q[1] p.orientation.z = q[2] p.orientation.w = q[3] return def update_particle_weights_with_measurement_model(self, data, log=False): for p in self.particle_cloud: p.w = 1 # four the four cardinal directions find scan distance and use to update for i in [0, 90, 180, 270]: scan_dist = data.ranges[i] # skip scan angles that did not detect anything if scan_dist == math.inf: continue robot_yaw = np.deg2rad(i) for p in self.particle_cloud: p_yaw = get_yaw_from_pose(p.pose) p_x = p.pose.position.x p_y = p.pose.position.y tot_yaw = p_yaw + robot_yaw s_x = p_x + scan_dist * math.cos(tot_yaw) s_y = p_y + scan_dist * math.sin(tot_yaw) # get closest obstacle using likelihood field dist = self.likelihood_field.get_closest_obstacle_distance( s_x, s_y) if not dist or math.isnan(dist): dist = 0 prob = compute_prob_zero_centered_gaussian(dist, 0.1) p.w = p.w * prob def update_particles_with_motion_model(self): # based on the how the robot has moved (calculated from its odometry), we'll move # all of the particles correspondingly # calculate movement by comparing current odometry with previous value prev = self.odom_pose_last_motion_update.pose curr = self.odom_pose.pose diff_x = curr.position.x - prev.position.x diff_y = curr.position.y - prev.position.y # calculate angle diff_angle = 0 if diff_y > 0 else math.pi if diff_x != 0: diff_angle = math.atan(diff_y / diff_x) if diff_x < 0: diff_angle += math.pi # calculate distance and angle dist = math.sqrt(math.pow(diff_x, 2) + math.pow(diff_y, 2)) angle1 = get_yaw_from_pose(prev) angle2 = get_yaw_from_pose(curr) angle_change = angle2 - angle1 # loop through particles updating their position with noise for part in self.particle_cloud: move_dist = dist + np.random.normal(0, 0.2) p_angle = get_yaw_from_pose(part.pose) move_angle = p_angle + (diff_angle - angle1) + np.random.normal( 0, 0.1) p = Pose() p.position.x = part.pose.position.x + move_dist * math.cos( move_angle) p.position.y = part.pose.position.y + move_dist * math.sin( move_angle) p.position.z = 0 q = quaternion_from_euler( 0.0, 0.0, p_angle + angle_change + np.random.normal(0, 0.2)) p.orientation.x = q[0] p.orientation.y = q[1] p.orientation.z = q[2] p.orientation.w = q[3] part.pose = p
class Pick(ScenarioStateBase): def __init__(self, save_sm_state=False, **kwargs): ScenarioStateBase.__init__(self, 'pickup', save_sm_state=save_sm_state, output_keys=['grasped_object'], outcomes=[ 'succeeded', 'failed', 'failed_after_retrying', 'find_objects_before_picking' ]) self.sm_id = kwargs.get('sm_id', 'mdr_demo_throw_table_objects') self.state_name = kwargs.get('state_name', 'pick') self.timeout = kwargs.get('timeout', 120.) self.tf_listener = TransformListener() self.number_of_retries = kwargs.get('number_of_retries', 0) self.retry_count = 0 def execute(self, userdata): if self.save_sm_state: self.save_current_state() surface_objects = self.get_surface_objects(surface_prefix='table') object_poses = self.get_object_poses(surface_objects) surface, obj_to_grasp_idx = self.select_object_for_grasping( object_poses) obj_to_grasp = '' if obj_to_grasp_idx != -1: obj_to_grasp = surface_objects[surface][obj_to_grasp_idx] else: rospy.logerr('Could not find an object to grasp') self.say('Could not find an object to grasp') return 'find_objects_before_picking' dispatch_msg = self.get_dispatch_msg(obj_to_grasp, surface) rospy.loginfo('Picking %s from %s' % (obj_to_grasp, surface)) self.say('Picking ' + obj_to_grasp + ' from ' + surface) self.action_dispatch_pub.publish(dispatch_msg) self.executing = True self.succeeded = False start_time = time.time() duration = 0. while self.executing and duration < self.timeout: rospy.sleep(0.1) duration = time.time() - start_time if self.succeeded: rospy.loginfo('%s grasped successfully' % obj_to_grasp) self.say('Successfully grasped ' + obj_to_grasp) userdata.grasped_object = obj_to_grasp return 'succeeded' rospy.loginfo('Could not grasp %s' % obj_to_grasp) self.say('Could not grasp ' + obj_to_grasp) if self.retry_count == self.number_of_retries: rospy.loginfo('Failed to grasp %s' % obj_to_grasp) self.say('Aborting operation') return 'failed_after_retrying' rospy.loginfo('Retrying to grasp %s' % obj_to_grasp) self.retry_count += 1 return 'failed' def get_surface_objects(self, surface_prefix='table'): surface_objects = dict() request = rosplan_srvs.GetAttributeServiceRequest() request.predicate_name = 'on' result = self.attribute_fetching_client(request) for item in result.attributes: object_on_desired_surface = False object_name = '' surface_name = '' if not item.is_negative: for param in item.values: if param.key == 'plane' and param.value.find( surface_prefix) != -1: object_on_desired_surface = True surface_name = param.value if surface_name not in surface_objects: surface_objects[surface_name] = list() elif param.key == 'obj': object_name = param.value if object_on_desired_surface: surface_objects[surface_name].append(object_name) return surface_objects def get_object_poses(self, surface_objects): object_poses = dict() for surface, objects in surface_objects.items(): object_poses[surface] = list() for obj_name in objects: try: obj = self.msg_store_client.query_named( obj_name, Object._type)[0] object_poses[surface].append(obj.pose) except: rospy.logerr('Error retriving knowledge about %s', obj_name) pass return object_poses def select_object_for_grasping(self, object_poses): '''Returns the index of the object whose distance is closest to the robot ''' object_surfaces = list() distances = dict() robot_position = np.zeros(3) for surface, poses in object_poses.items(): object_surfaces.append(surface) distances[surface] = list() for pose in poses: base_link_pose = self.tf_listener.transformPose( 'base_link', pose) distances[surface].append( self.distance( robot_position, np.array([ base_link_pose.pose.position.x, base_link_pose.pose.position.y, base_link_pose.pose.position.z ]))) min_dist = 1e100 min_dist_obj_idx = -1 obj_surface = '' for surface, distance_list in distances.items(): if distance_list: surface_min_dist = np.min(distance_list) if surface_min_dist < min_dist: min_dist = surface_min_dist min_dist_obj_idx = np.argmin(distance_list) obj_surface = surface return obj_surface, min_dist_obj_idx def get_robot_pose(self, map_frame='map', base_link_frame='base_link'): latest_tf_time = self.tf_listener.getLatestCommonTime( base_link_frame, map_frame) position, quat_orientation = self.tf_listener.lookupTransform( base_link_frame, map_frame, latest_tf_time) return position, quat_orientation def distance(self, point1, point2): return np.linalg.norm(np.array(point1) - np.array(point2)) def get_dispatch_msg(self, obj_name, surface_name): dispatch_msg = plan_dispatch_msgs.ActionDispatch() dispatch_msg.name = self.action_name arg_msg = diag_msgs.KeyValue() arg_msg.key = 'bot' arg_msg.value = self.robot_name dispatch_msg.parameters.append(arg_msg) arg_msg = diag_msgs.KeyValue() arg_msg.key = 'obj' arg_msg.value = obj_name dispatch_msg.parameters.append(arg_msg) arg_msg = diag_msgs.KeyValue() arg_msg.key = 'plane' arg_msg.value = surface_name dispatch_msg.parameters.append(arg_msg) return dispatch_msg
class NormalApproach(): standoff = 0.368 #0.2 + 0.168 (dist from wrist to fingertips) frame = 'base_footprint' px = py = pz = 0; qx = qy = qz = 0; qw = 1; def __init__(self): rospy.init_node('reactive_grasp_right') rospy.Subscriber('reactive_grasp_right', PoseStamped, self.update_frame) rospy.Subscriber('r_hand_pose', PoseStamped, self.update_curr_pose) rospy.Subscriber('wt_lin_move_right',Float32, self.linear_move) self.goal_out = rospy.Publisher('/reactive_grasp/right/goal', ReactiveGraspActionGoal) self.test_out = rospy.Publisher('/right_test_pose', PoseStamped) self.move_arm_out = rospy.Publisher('wt_move_right_arm_goals', PoseStamped) self.tf = TransformListener() self.tfb = TransformBroadcaster() self.wt_log_out = rospy.Publisher('wt_log_out', String ) def update_curr_pose(self, msg): self.currpose = msg; def reactive_grasp(self, ps): rospy.loginfo("Calling Reactive Grasp") rg_goal = ReactiveGraspActionGoal() rg_goal.goal.arm_name = 'right_arm' rg_goal.goal.final_grasp_pose = ps self.test_out.publish(ps) self.goal_out.publish(rg_goal) def update_frame(self, pose): rospy.loginfo("Updating Frame") self.standoff = 0.368 self.frame = pose.header.frame_id self.px = pose.pose.position.x self.py = pose.pose.position.y self.pz = pose.pose.position.z self.qx = pose.pose.orientation.x self.qy = pose.pose.orientation.y self.qz = pose.pose.orientation.z self.qw = pose.pose.orientation.w self.tfb.sendTransform((self.px,self.py,self.pz),(self.qx,self.qy,self.qz,self.qw), rospy.Time.now(), "pixel_3d_frame", self.frame) self.find_approach(pose) def find_approach(self, msg): rospy.loginfo("Finding Approach Point") self.pose_in = msg self.tf.waitForTransform('pixel_3d_frame','base_footprint', rospy.Time(0), rospy.Duration(3.0)) self.tfb.sendTransform((self.pose_in.pose.position.x, self.pose_in.pose.position.y, self.pose_in.pose.position.z),(self.pose_in.pose.orientation.x, self.pose_in.pose.orientation.y, self.pose_in.pose.orientation.z, self.pose_in.pose.orientation.w), rospy.Time.now(), "pixel_3d_frame", self.pose_in.header.frame_id) self.tf.waitForTransform('pixel_3d_frame','r_wrist_roll_link', rospy.Time.now(), rospy.Duration(3.0)) goal = PoseStamped() goal.header.frame_id = 'pixel_3d_frame' goal.header.stamp = rospy.Time.now() goal.pose.position.x = 0 goal.pose.position.y = 0 goal.pose.position.z = 0.1 goal.pose.orientation.x = 0 goal.pose.orientation.y = 0.5*math.sqrt(2) goal.pose.orientation.z = 0 goal.pose.orientation.w = 0.5*math.sqrt(2) #print "Goal:\r\n %s" %goal self.tf.waitForTransform(goal.header.frame_id, 'torso_lift_link', rospy.Time.now(), rospy.Duration(3.0)) appr = self.tf.transformPose('torso_lift_link', goal) # print "Appr: \r\n %s" %appr self.wt_log_out.publish(data="Normal Approach with right hand: Trying to move WITH motion planning") #self.move_arm_out.publish(appr) self.reactive_grasp(appr) def linear_move(self, msg): print "Linear Move: Right Arm: %s m Step" %msg.data self.tf.waitForTransform(self.currpose.header.frame_id, 'r_wrist_roll_link', self.currpose.header.stamp, rospy.Duration(3.0)) newpose = self.tf.transformPose('r_wrist_roll_link', self.currpose) newpose.pose.position.x += msg.data step_goal = self.tf.transformPose(self.currpose.header.frame_id, newpose) self.goal_out.publish(step_goal)
class OccupancyGridMapper: """ Implements simple occupancy grid mapping """ def __init__(self): cv2.namedWindow("map") #Establish mapping conventions self.origin = [-10, -10] self.seq = 0 self.resolution = .1 self.n = 200 self.p_occ = .5 self.odds_ratio_hit = 3.0 self.odds_ratio_miss = .2 self.odds_ratios = self.p_occ / (1 - self.p_occ) * np.ones((self.n, self.n)) self.tf_listener = TransformListener() #Subscribers and Publishers rospy.Subscriber("scan", LaserScan, self.process_scan, queue_size=1) self.pub = rospy.Publisher("map", OccupancyGrid) self.ycoor_pub = rospy.Publisher("/yellow_coords", Vector3) self.bcoor_pub = rospy.Publisher("/blue_coords", Vector3) self.rcoor_pub = rospy.Publisher("/red_coords", Vector3) self.gcoor_pub = rospy.Publisher("/green_coords", Vector3) self.coords_sub_red = rospy.Subscriber('ball_coords_red', Vector3, self.coordinate_to_map_red) self.coords_sub_green = rospy.Subscriber('ball_coords_green', Vector3, self.coordinate_to_map_green) self.coords_sub_blue = rospy.Subscriber('ball_coords_blue', Vector3, self.coordinate_to_map_blue) self.coords_sub_yellow = rospy.Subscriber('ball_coords_yellow', Vector3, self.coordinate_to_map_yellow) #Camera translations #TODO Add stuff for each color, so can map more than one at a time self.frame_height = 480 self.frame_width = 640 self.depth_proportion = -0.013 self.depth_intercept = 2.105 #85pixels - 1meter #62pixels - 1.3meter self.red = (0, 0, 255) self.yellow = (0, 255, 255) self.blue = (255, 0, 0) self.green = (0, 255, 0) self.depth_yellow = 0 self.y_transform_yellow = 0 self.x_transform_yellow = 0 self.angle_diff_yellow = 0 self.depth_red = 0 self.y_transform_red = 0 self.x_transform_red = 0 self.angle_diff_red = 0 self.depth_green = 0 self.y_transform_green = 0 self.x_transform_green = 0 self.angle_diff_green = 0 self.depth_blue = 0 self.y_transform_blue = 0 self.x_transform_blue = 0 self.angle_diff_blue = 0 self.x_camera_red = -1 self.y_camera_red = -1 self.x_camera_blue = -1 self.y_camera_blue = -1 self.x_camera_green = -1 self.y_camera_green = -1 self.x_camera_yellow = -1 self.y_camera_yellow = -1 rospy.loginfo('Mapper running') def is_in_map(self, x_ind, y_ind): """ Return whether or not the given point is within the map boundaries """ return not (x_ind < self.origin[0] or x_ind > self.origin[0] + self.n * self.resolution or y_ind < self.origin[1] or y_ind > self.origin[1] + self.n * self.resolution) def process_scan(self, msg): """ Callback function for the laser scan messages """ if len(msg.ranges) <= 330: # throw out scans that don't have more than 90% of the data return # get pose according to the odometry p = PoseStamped(header=Header(stamp=msg.header.stamp, frame_id="base_link"), pose=Pose()) self.odom_pose = self.tf_listener.transformPose("odom", p) self.base_pose = self.tf_listener.transformPose("base_laser_link", p) # convert the odom pose to the tuple (x,y,theta) self.odom_pose = OccupancyGridMapper.convert_pose_to_xy_and_theta(self.odom_pose.pose) #(-0.0069918, 0.000338577, 0.048387097) #(1.0208817, 0.04827240, 0.048387) self.base_pose = OccupancyGridMapper.convert_pose_to_xy_and_theta(self.base_pose.pose) for i in range(len(msg.ranges)): if 0.0 < msg.ranges[i] < 5.0: #for any reding within 5 meters #Using the pose and the measurement nd the angle, find it in the world map_x = self.odom_pose[0] + msg.ranges[i] * cos(i * pi / 180.0 + self.odom_pose[2]) map_y = self.odom_pose[1] + msg.ranges[i] * sin(i * pi / 180.0 + self.odom_pose[2]) #Relate that map measure with a place in the picture x_detect = int((map_x - self.origin[0]) / self.resolution) y_detect = int((map_y - self.origin[1]) / self.resolution) #Determine how to mark the location in the map, along with the stuff inbetween u = (map_x - self.odom_pose[0], map_y - self.odom_pose[1]) magnitude = sqrt(u[0] ** 2 + u[1] ** 2) n_steps = max([1, int(ceil(magnitude / self.resolution))]) u_step = (u[0] / (n_steps - 1), u[1] / (n_steps - 1)) marked = set() for i in range(n_steps): curr_x = self.odom_pose[0] + i * u_step[0] curr_y = self.odom_pose[1] + i * u_step[1] if not (self.is_in_map(curr_x, curr_y)): break x_ind = int((curr_x - self.origin[0]) / self.resolution) y_ind = int((curr_y - self.origin[1]) / self.resolution) if x_ind == x_detect and y_ind == y_detect: break if not ((x_ind, y_ind) in marked): # odds ratio is updated according to the inverse sensor model self.odds_ratios[x_ind, y_ind] *= self.p_occ / (1 - self.p_occ) * self.odds_ratio_miss marked.add((x_ind, y_ind)) if self.is_in_map(map_x, map_y): # odds ratio is updated according to the inverse sensor model self.odds_ratios[x_detect, y_detect] *= self.p_occ / (1 - self.p_occ) * self.odds_ratio_hit self.seq += 1 # to save time, only publish the map every 10 scans that we process if self.seq % 10 == 0: # make occupancy grid map = OccupancyGrid() map.header.seq = self.seq self.seq += 1 map.header.stamp = msg.header.stamp map.header.frame_id = "map" # the name of the coordinate frame of the map map.info.origin.position.x = self.origin[0] map.info.origin.position.y = self.origin[1] map.info.width = self.n map.info.height = self.n map.info.resolution = self.resolution map.data = [0] * self.n ** 2 # map.data stores the n by n grid in row-major order for i in range(self.n): for j in range(self.n): idx = i + self.n * j # this implements row major order if self.odds_ratios[i, j] < 1 / 5.0: # consider a cell free if odds ratio is low enough map.data[idx] = 0 elif self.odds_ratios[i, j] > 5.0: # consider a cell occupied if odds ratio is high enough map.data[idx] = 100 else: # otherwise cell is unknown map.data[idx] = -1 self.pub.publish(map) # create the image from the probabilities so we can visualize using opencv im = np.zeros((self.odds_ratios.shape[0], self.odds_ratios.shape[1], 3)) for i in range(im.shape[0]): for j in range(im.shape[1]): if self.odds_ratios[i, j] < 1 / 5.0: im[i, j, :] = 1.0 elif self.odds_ratios[i, j] > 5.0: im[i, j, :] = 0.0 else: im[i, j, :] = 0.5 # compute the index of the odometry pose so we can mark it with a circle x_odom_index = int((self.odom_pose[0] - self.origin[0]) / self.resolution) y_odom_index = int((self.odom_pose[1] - self.origin[1]) / self.resolution) x_base_index = int((self.base_pose[0] - self.origin[0] - 1) / self.resolution) y_base_index = int((self.base_pose[1] - self.origin[1]) / self.resolution) # computer the ball locations so we can mark with a colored circle #TODO Track and relate the robot's angle pose for accuracy if self.depth_red > 0: self.y_camera_red = int(x_odom_index - self.depth_red * cos(self.angle_diff_red + pi - self.odom_pose[2])/self.resolution) self.x_camera_red = int(y_odom_index - self.depth_red * -sin(self.angle_diff_red + pi - self.odom_pose[2])/self.resolution) cv2.circle(im, (self.x_camera_red, self.y_camera_red), 1, self.red) real_red_y = self.depth_red * cos(self.angle_diff_red + pi - self.odom_pose[2]) real_red_x = self.depth_red * -sin(self.angle_diff_red + pi - self.odom_pose[2]) self.rcoor_pub.publish(Vector3(-real_red_x, -real_red_y/2, 0)) else: cv2.circle(im, (self.x_camera_red, self.y_camera_red), 1, self.red) if self.depth_blue > 0: self.y_camera_blue = int(x_odom_index - self.depth_blue * cos(self.angle_diff_blue + pi - self.odom_pose[2])/self.resolution) self.x_camera_blue = int(y_odom_index - self.depth_blue * -sin(self.angle_diff_blue + pi - self.odom_pose[2])/self.resolution) cv2.circle(im, (self.x_camera_blue, self.y_camera_blue), 1, self.blue) real_blue_y = self.depth_blue * cos(self.angle_diff_blue + pi - self.odom_pose[2]) real_blue_x = self.depth_blue * -sin(self.angle_diff_blue + pi - self.odom_pose[2]) self.bcoor_pub.publish(Vector3(-real_blue_x, -real_blue_y/2, 0)) else: cv2.circle(im, (self.x_camera_blue, self.y_camera_blue), 1, self.blue) if self.depth_green > 0: self.y_camera_green = int(x_odom_index - self.depth_green * cos(self.angle_diff_green + pi - self.odom_pose[2])/self.resolution) self.x_camera_green = int(y_odom_index - self.depth_green * -sin(self.angle_diff_green + pi - self.odom_pose[2])/self.resolution) cv2.circle(im, (self.x_camera_green, self.y_camera_green), 1, self.green) real_green_y = self.depth_green * cos(self.angle_diff_green + pi - self.odom_pose[2]) real_green_x = self.depth_green * -sin(self.angle_diff_green + pi - self.odom_pose[2]) self.gcoor_pub.publish(Vector3(-real_green_x, -real_green_y/2, 0)) else: cv2.circle(im, (self.x_camera_green, self.y_camera_green), 1, self.green) if self.depth_yellow > 0: self.y_camera_yellow = int(x_odom_index - self.depth_yellow * cos(self.angle_diff_yellow + pi - self.odom_pose[2])/self.resolution) self.x_camera_yellow = int(y_odom_index - self.depth_yellow * -sin(self.angle_diff_yellow + pi - self.odom_pose[2])/self.resolution) cv2.circle(im, (self.x_camera_yellow, self.y_camera_yellow), 1, self.yellow) real_yellow_y = self.depth_yellow * cos(self.angle_diff_yellow + pi - self.odom_pose[2]) real_yellow_x = self.depth_yellow * -sin(self.angle_diff_yellow + pi - self.odom_pose[2]) self.ycoor_pub.publish(Vector3(-real_yellow_x, -real_yellow_y/2, 0)) else: cv2.circle(im, (self.x_camera_yellow, self.y_camera_yellow), 1, self.yellow) # draw the robot cv2.circle(im, (y_odom_index, x_odom_index), 2, (255, 0, 0)) # display the image resized cv2.imshow("map", cv2.resize(im, (500, 500))) cv2.waitKey(20) def coordinate_to_map_red(self, msg): x = msg.x y = msg.y r = msg.z if r != 0: self.depth_red = (r * self.depth_proportion + self.depth_intercept) #print depth self.y_transform_red = int(self.frame_height / 2 - y) self.x_transform_red = int(x - self.frame_width / 2) / 100 self.angle_diff_red = self.x_transform_red * pi/180.0 else: self.depth_red = 0 def coordinate_to_map_green(self, msg): x = msg.x y = msg.y r = msg.z if r != 0: self.depth_green = (r * self.depth_proportion + self.depth_intercept) #print depth self.y_transform_green = int(self.frame_height / 2 - y) self.x_transform_green = int(x - self.frame_width / 2) / 100 self.angle_diff_green = self.x_transform_green * pi/180.0 else: self.depth_green = 0 def coordinate_to_map_blue(self, msg): x = msg.x y = msg.y r = msg.z if r != 0: self.depth_blue = (r * self.depth_proportion + self.depth_intercept) #print depth self.y_transform_blue = int(self.frame_height / 2 - y) self.x_transform_blue = int(x - self.frame_width / 2) / 100 self.angle_diff_blue = self.x_transform_blue * pi/180.0 else: self.depth_blue = 0 def coordinate_to_map_yellow(self, msg): x = msg.x y = msg.y r = msg.z if r != 0: self.depth_yellow = (r * self.depth_proportion + self.depth_intercept) #print depth self.y_transform_yellow = int(self.frame_height / 2 - y) self.x_transform_yellow = int(x - self.frame_width / 2) / 100 self.angle_diff_yellow = self.x_transform_yellow * pi/180.0 else: self.depth_yellow = 0 @staticmethod def convert_pose_to_xy_and_theta(pose): """ Convert pose (geometry_msgs.Pose) to a (x,y,yaw) tuple """ orientation_tuple = (pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w) angles = euler_from_quaternion(orientation_tuple) return pose.position.x, pose.position.y, angles[2]
class ParticleFilter: def __init__(self): # once everything is setup initialized will be set to true self.initialized = False # initialize this particle filter node rospy.init_node('turtlebot3_particle_filter') # set the topic names and frame names self.base_frame = "base_footprint" self.map_topic = "map" self.odom_frame = "odom" self.scan_topic = "scan" # inialize our map self.map = OccupancyGrid() # haha are we supposed to do this part 3 self.likelihood_field = None # the number of particles used in the particle filter self.num_particles = 10000 # initialize the particle cloud array self.particle_cloud = [] # initialize the estimated robot pose self.robot_estimate = Pose() # set threshold values for linear and angular movement before we preform an update self.lin_mvmt_threshold = 0.2 self.ang_mvmt_threshold = (np.pi / 6) self.odom_pose_last_motion_update = None # Setup publishers and subscribers # publish the current particle cloud self.particles_pub = rospy.Publisher("particle_cloud", PoseArray, queue_size=10) # publish the estimated robot pose self.robot_estimate_pub = rospy.Publisher("estimated_robot_pose", PoseStamped, queue_size=10) # subscribe to the map server rospy.Subscriber(self.map_topic, OccupancyGrid, self.get_map) rospy.sleep(1) #self.likelihood_field = LikelihoodField() # subscribe to the lidar scan from the robot rospy.Subscriber(self.scan_topic, LaserScan, self.robot_scan_received) # enable listening for and broadcasting corodinate transforms self.tf_listener = TransformListener() self.tf_broadcaster = TransformBroadcaster() # intialize the particle cloud rospy.sleep(2) self.initialize_particle_cloud() self.initialized = True def get_map(self, data): self.likelihood_field = LikelihoodField(data) self.map = data def initialize_particle_cloud(self): # Figure out height and width of map width = self.map.info.width height = self.map.info.height # Map is a 384 x 384 grid # Create a new array that stores the locations of all points on map that are empty full_array = [] for i in range(width): for j in range(height): indx = (i * width + j) if self.map.data[indx] == 0: full_array.append(indx) # Set all our initial particles to empty initial_particle_set = [] # Create a random sample of size n from all the empty locations on map new_sample = rand.sample(full_array, self.num_particles) # Tranform each point into real coordinates and add a randomized orientation for part in new_sample: # Find the origin and resolution of the map # Recalculate the x and y values of the randomly sampled points to fit our map resolution = self.map.info.resolution x_origin = self.map.info.origin.position.x y_origin = self.map.info.origin.position.y part = convert_to_real_coords(part, width, x_origin, y_origin, resolution) # Randomly choose an orientation for each particle between 0 and 2 pi rand_orientation = math.radians(randint(0, 359)) part.append(rand_orientation) # Add particle to our initial particle set initial_particle_set.append(part) # Initialize our particle cloud to be the size of our map self.particle_cloud = [] # Use code from class to convert to particle type for i in range(len(initial_particle_set)): p = Pose() p.position = Point() p.position.x = initial_particle_set[i][0] p.position.y = initial_particle_set[i][1] p.position.z = 0 p.orientation = Quaternion() q = quaternion_from_euler(0.0, 0.0, initial_particle_set[i][2]) p.orientation.x = q[0] p.orientation.y = q[1] p.orientation.z = q[2] p.orientation.w = q[3] # Initialize the new particle, where all will have the same weight (1.0) new_particle = Particle(p, 1.0) # Append the particle to the particle cloud self.particle_cloud.append(new_particle) # END self.normalize_particles() self.publish_particle_cloud() def normalize_particles(self): # Make all the particle weights sum to 1.0 # Create sum object to hold total weights sum = 0 for part in self.particle_cloud: sum += part.w # Make sure we dont divide by zero if sum != 0: # Re-weigh each particle (normalize them) for part in self.particle_cloud: part.w = part.w / sum def publish_particle_cloud(self): rospy.sleep(1) particle_cloud_pose_array = PoseArray() particle_cloud_pose_array.header = Header(stamp=rospy.Time.now(), frame_id=self.map_topic) particle_cloud_pose_array.poses for part in self.particle_cloud: particle_cloud_pose_array.poses.append(part.pose) self.particles_pub.publish(particle_cloud_pose_array) def publish_estimated_robot_pose(self): robot_pose_estimate_stamped = PoseStamped() robot_pose_estimate_stamped.pose = self.robot_estimate robot_pose_estimate_stamped.header = Header(stamp=rospy.Time.now(), frame_id=self.map_topic) self.robot_estimate_pub.publish(robot_pose_estimate_stamped) def resample_particles(self): # Create 1D array of weights to feed to our draw_random_sample function weights = [] for part in self.particle_cloud: weights.append(part.w) # Use draw_random_sample to create a new particle cloud self.particle_cloud = draw_random_sample(self.particle_cloud, np.array(weights), self.num_particles) def robot_scan_received(self, data): # wait until initialization is complete if not (self.initialized): return # we need to be able to transfrom the laser frame to the base frame if not (self.tf_listener.canTransform( self.base_frame, data.header.frame_id, data.header.stamp)): return # wait for a little bit for the transform to become avaliable (in case the scan arrives # a little bit before the odom to base_footprint transform was updated) self.tf_listener.waitForTransform(self.base_frame, self.odom_frame, data.header.stamp, rospy.Duration(0.5)) if not (self.tf_listener.canTransform( self.base_frame, data.header.frame_id, data.header.stamp)): return # calculate the pose of the laser distance sensor p = PoseStamped( header=Header(stamp=rospy.Time(0), frame_id=data.header.frame_id)) self.laser_pose = self.tf_listener.transformPose(self.base_frame, p) # determine where the robot thinks it is based on its odometry #use this important p = PoseStamped(header=Header(stamp=data.header.stamp, frame_id=self.base_frame), pose=Pose()) self.odom_pose = self.tf_listener.transformPose(self.odom_frame, p) # we need to be able to compare the current odom pose to the prior odom pose # if there isn't a prior odom pose, set the odom_pose variable to the current pose #this is how far robot has moved if not self.odom_pose_last_motion_update: self.odom_pose_last_motion_update = self.odom_pose return if self.particle_cloud: # check to see if we've moved far enough to perform an update curr_x = self.odom_pose.pose.position.x old_x = self.odom_pose_last_motion_update.pose.position.x curr_y = self.odom_pose.pose.position.y old_y = self.odom_pose_last_motion_update.pose.position.y curr_yaw = get_yaw_from_pose(self.odom_pose.pose) old_yaw = get_yaw_from_pose(self.odom_pose_last_motion_update.pose) if (np.abs(curr_x - old_x) > self.lin_mvmt_threshold or np.abs(curr_y - old_y) > self.lin_mvmt_threshold or np.abs(curr_yaw - old_yaw) > self.ang_mvmt_threshold): # This is where the main logic of the particle filter is carried out self.update_particles_with_motion_model() self.update_particle_weights_with_measurement_model(data) self.normalize_particles() self.resample_particles() self.update_estimated_robot_pose() self.publish_particle_cloud() self.publish_estimated_robot_pose() self.odom_pose_last_motion_update = self.odom_pose def update_estimated_robot_pose(self): # Based on the particles within the particle cloud, update the robot pose estimate # Use average location of all particles # Find total x and y locations totalx = 0 totaly = 0 total_yaw = 0 for part in self.particle_cloud: # Accumulate position pos = part.pose.position totalx += pos.x totaly += pos.y # Accumulate orientation p = part.pose total_yaw += get_yaw_from_pose(p) # Calculate new locations new_x = totalx / self.num_particles new_y = totaly / self.num_particles # Calculate new orientation new_yaw = total_yaw / self.num_particles new_quat = quaternion_from_euler(0.0, 0.0, new_yaw) # Set new robot position estimate self.robot_estimate.position.x = new_x self.robot_estimate.position.y = new_y # Set new robot orientation estimate self.robot_estimate.orientation.z = new_quat[2] self.robot_estimate.orientation.w = new_quat[3] def update_particle_weights_with_measurement_model(self, data): # Check that likelihood_field is loaded from map if self.likelihood_field == None: return else: # Should use this for full list of directions allDirections = range(360) # Set cardinal directions cardinal_directions_idxs = [0, 90, 180, 270] # Code taken from in-class exercise for part in self.particle_cloud: q = 1 for direction in cardinal_directions_idxs: theta = get_yaw_from_pose(part.pose) ztk = data.ranges[direction] if (data.ranges[direction] <= 3.5): xztk = part.pose.position.x + ( ztk * math.cos(theta + math.radians(direction))) yztk = part.pose.position.y + ( ztk * math.sin(theta + math.radians(direction))) dist = self.likelihood_field.get_closest_obstacle_distance( xztk, yztk) prob = compute_prob_zero_centered_gaussian(dist, 0.1) # Only modify q if the probability is not a nan value if not (math.isnan(prob)): q = q * prob # If q is a nan value or 1 (i.e. hasnt been changed at all), set q to 0 if (math.isnan(q)) or q == 1: q = 0 # Update particle weight part.w = q def update_particles_with_motion_model(self): # Find old and new robot position and orientation # Calculate distance moved in x direction x_old = self.odom_pose_last_motion_update.pose.position.x x_new = self.odom_pose.pose.position.x delta_x = x_new - x_old # Calculate distance moved in y direction y_old = self.odom_pose_last_motion_update.pose.position.y y_new = self.odom_pose.pose.position.y delta_y = y_new - y_old # Calculate rotation yaw_old = get_yaw_from_pose(self.odom_pose_last_motion_update.pose) yaw_new = get_yaw_from_pose(self.odom_pose.pose) delta_yaw = yaw_new - yaw_old # Calculate total distance moved using pythagorean thm total_dist = math.sqrt(delta_x * delta_x + delta_y * delta_y) # Based on the how the robot has moved (calculated from its odometry), # move all of the particles correspondingly # Modify each particle direction and orientation based on how robot has moved # Move each particle the same distance and direction as robot respective to its orientation for part in self.particle_cloud: # Find particle orientation and add noise theta = get_yaw_from_pose(part.pose) theta_with_noise = np.random.normal(loc=(theta), scale=0.2) # Add the robots direction change and convert back to a quaternion quat_noise = quaternion_from_euler(0, 0, theta_with_noise + delta_yaw) # Calculate the angle between the robot and particles orientation and # Calculate the respective x and y directions using that angle theta_diff = theta - yaw_old new_x = (delta_x * math.cos(theta_diff)) + (delta_y * math.sin(theta_diff)) new_y = (-delta_x * math.sin(theta_diff)) + (delta_y * math.cos(theta_diff)) # Update the particles location and orientation part.pose.position.x += np.random.normal(loc=new_x, scale=0.2) part.pose.position.y += np.random.normal(loc=new_y, scale=0.2) part.pose.orientation.x = quat_noise[0] part.pose.orientation.y = quat_noise[1] part.pose.orientation.z = quat_noise[2] part.pose.orientation.w = quat_noise[3]
class ArmCommander(Limb): """ This class overloads Limb from the Baxter Python SDK adding the support of trajectories via RobotState and RobotTrajectory messages Allows to control the entire arm either in joint space, or in task space, or (later) with path planning, all with simulation """ def __init__(self, name, rate=100, fk='robot', ik='trac', default_kv_max=1., default_ka_max=0.5): """ :param name: 'left' or 'right' :param rate: Rate of the control loop for execution of motions :param fk: Name of the Forward Kinematics solver, "robot", "kdl", "trac" or "ros" :param ik: Name of the Inverse Kinematics solver, "robot", "kdl", "trac" or "ros" :param default_kv_max: Default K maximum for velocity :param default_ka_max: Default K maximum for acceleration """ Limb.__init__(self, name) self._world = 'base' self.kv_max = default_kv_max self.ka_max = default_ka_max self._gripper = Gripper(name) self._rate = rospy.Rate(rate) self._tf_listener = TransformListener() self.recorder = Recorder(name) # Kinematics services: names and services (if relevant) self._kinematics_names = {'fk': {'ros': 'compute_fk'}, 'ik': {'ros': 'compute_ik', 'robot': 'ExternalTools/{}/PositionKinematicsNode/IKService'.format(name), 'trac': 'trac_ik_{}'.format(name)}} self._kinematics_services = {'fk': {'ros': {'service': rospy.ServiceProxy(self._kinematics_names['fk']['ros'], GetPositionFK), 'func': self._get_fk_ros}, 'kdl': {'func': self._get_fk_pykdl}, 'robot': {'func': self._get_fk_robot}}, 'ik': {'ros': {'service': rospy.ServiceProxy(self._kinematics_names['ik']['ros'], GetPositionIK), 'func': self._get_ik_ros}, 'robot': {'service': rospy.ServiceProxy(self._kinematics_names['ik']['robot'], SolvePositionIK), 'func': self._get_ik_robot}, 'trac': {'service': rospy.ServiceProxy(self._kinematics_names['ik']['trac'], GetConstrainedPositionIK), 'func': self._get_ik_trac}, 'kdl': {'func': self._get_ik_pykdl}}} self._selected_ik = ik self._selected_fk = fk # Kinematics services: PyKDL self._kinematics_pykdl = baxter_kinematics(name) if self._selected_ik in self._kinematics_names['ik']: rospy.wait_for_service(self._kinematics_names['ik'][self._selected_ik]) if self._selected_fk in self._kinematics_names['fk']: rospy.wait_for_service(self._kinematics_names['fk'][self._selected_fk]) # Execution attributes rospy.Subscriber('/robot/limb/{}/collision_detection_state'.format(name), CollisionDetectionState, self._cb_collision, queue_size=1) rospy.Subscriber('/robot/digital_io/{}_lower_cuff/state'.format(name), DigitalIOState, self._cb_dig_io, queue_size=1) self._stop_reason = '' # 'cuff' or 'collision' could cause a trajectory to be stopped self._stop_lock = Lock() action_server_name = "/robot/limb/{}/follow_joint_trajectory".format(self.name) self.client = SimpleActionClient(action_server_name, FollowJointTrajectoryAction) self._display_traj = rospy.Publisher("/move_group/display_planned_path", DisplayTrajectory, queue_size=1) self._gripper.calibrate() self.client.wait_for_server() ######################################### CALLBACKS ######################################### def _cb_collision(self, msg): if msg.collision_state: with self._stop_lock: self._stop_reason = 'collision' def _cb_dig_io(self, msg): if msg.state > 0: with self._stop_lock: self._stop_reason = 'cuff' ############################################################################################# def endpoint_pose(self): """ Returns the pose of the end effector :return: [[x, y, z], [x, y, z, w]] """ pose = Limb.endpoint_pose(self) return [[pose['position'].x, pose['position'].y, pose['position'].z], [pose['orientation'].x, pose['orientation'].y, pose['orientation'].z, pose['orientation'].w]] def endpoint_name(self): return self.name+'_gripper' def group_name(self): return self.name+'_arm' def joint_limits(self): xml_urdf = rospy.get_param('robot_description') dict_urdf = xmltodict.parse(xml_urdf) joints_urdf = [] joints_urdf.append([j['@name'] for j in dict_urdf['robot']['joint'] if j['@name'] in self.joint_names()]) joints_urdf.append([[float(j['limit']['@lower']), float(j['limit']['@upper'])] for j in dict_urdf['robot']['joint'] if j['@name'] in self.joint_names()]) # reorder the joints limits return dict(zip(self.joint_names(), [joints_urdf[1][joints_urdf[0].index(name)] for name in self.joint_names()])) def get_current_state(self, list_joint_names=[]): """ Returns the current RobotState describing all joint states :param list_joint_names: If not empty, returns only the state of the requested joints :return: a RobotState corresponding to the current state read on /robot/joint_states """ if len(list_joint_names) == 0: list_joint_names = self.joint_names() state = RobotState() state.joint_state.name = list_joint_names state.joint_state.position = map(self.joint_angle, list_joint_names) state.joint_state.velocity = map(self.joint_velocity, list_joint_names) state.joint_state.effort = map(self.joint_effort, list_joint_names) return state def get_ik(self, eef_poses, seeds=(), source=None, params=None): """ Return IK solutions of this arm's end effector according to the method declared in the constructor :param eef_poses: a PoseStamped or a list [[x, y, z], [x, y, z, w]] in world frame or a list of PoseStamped :param seeds: a single seed or a list of seeds of type RobotState for each input pose :param source: 'robot', 'trac', 'kdl'... the IK source for this call (warning: the source might not be instanciated) :param params: dictionary containing optional non-generic IK parameters :return: a list of RobotState for each input pose in input or a single RobotState TODO: accept also a Point (baxter_pykdl's IK accepts orientation=None) Child methods wait for a *list* of pose(s) and a *list* of seed(s) for each pose """ if not isinstance(eef_poses, list) or isinstance(eef_poses[0], list) and not isinstance(eef_poses[0][0], list): eef_poses = [eef_poses] if not seeds: seeds=[] elif not isinstance(seeds, list): seeds = [seeds]*len(eef_poses) input = [] for eef_pose in eef_poses: if isinstance(eef_pose, list): input.append(list_to_pose(eef_pose, self._world)) elif isinstance(eef_pose, PoseStamped): input.append(eef_pose) else: raise TypeError("ArmCommander.get_ik() accepts only a list of Postamped or [[x, y, z], [x, y, z, w]], got {}".format(str(type(eef_pose)))) output = self._kinematics_services['ik'][self._selected_ik if source is None else source]['func'](input, seeds, params) return output if len(eef_poses)>1 else output[0] def get_fk(self, frame_id=None, robot_state=None): """ Return The FK solution oth this robot state according to the method declared in the constructor robot_state = None will give the current endpoint pose in frame_id :param robot_state: a RobotState message :param frame_id: the frame you want the endpoint pose into :return: [[x, y, z], [x, y, z, w]] """ if frame_id is None: frame_id = self._world if isinstance(robot_state, RobotState) or robot_state is None: return self._kinematics_services['fk'][self._selected_fk]['func'](frame_id, robot_state) else: raise TypeError("ArmCommander.get_fk() accepts only a RobotState, got {}".format(str(type(robot_state)))) def _get_fk_pykdl(self, frame_id, state=None): if state is None: state = self.get_current_state() fk = self._kinematics_pykdl.forward_position_kinematics(dict(zip(state.joint_state.name, state.joint_state.position))) return [fk[:3], fk[-4:]] def _get_fk_robot(self, frame_id = None, state=None): # Keep this half-working FK, still used by generate_cartesian_path (trajectories.py) if state is not None: raise NotImplementedError("_get_fk_robot has no FK service provided by the robot except for its current endpoint pose") ps = list_to_pose(self.endpoint_pose(), self._world) return self._tf_listener.transformPose(frame_id, ps) def _get_fk_ros(self, frame_id = None, state=None): rqst = GetPositionFKRequest() rqst.header.frame_id = self._world if frame_id is None else frame_id rqst.fk_link_names = [self.endpoint_name()] if isinstance(state, RobotState): rqst.robot_state = state elif isinstance(state, JointState): rqst.robot_state.joint_state = state elif state is None: rqst.robot_state = self.get_current_state() else: raise AttributeError("Provided state is an invalid type") fk_answer = self._kinematics_services['fk']['ros']['service'].call(rqst) if fk_answer.error_code.val==1: return fk_answer.pose_stamped[0] else: return None def _get_ik_pykdl(self, eef_poses, seeds=(), params=None): solutions = [] for pose_num, eef_pose in enumerate(eef_poses): if eef_pose.header.frame_id.strip('/') != self._world.strip('/'): raise NotImplementedError("_get_ik_pykdl: Baxter PyKDL implementation does not accept frame_ids other than {}".format(self._world)) pose = pose_to_list(eef_pose) resp = self._kinematics_pykdl.inverse_kinematics(pose[0], pose[1], [seeds[pose_num].joint_state.position[seeds[pose_num].joint_state.name.index(joint)] for joint in self.joint_names()] if len(seeds)>0 else None) if resp is None: rs = None else: rs = RobotState() rs.is_diff = False rs.joint_state.name = self.joint_names() rs.joint_state.position = resp solutions.append(rs) return solutions def _get_ik_robot(self, eef_poses, seeds=(), params=None): ik_req = SolvePositionIKRequest() for eef_pose in eef_poses: ik_req.pose_stamp.append(eef_pose) ik_req.seed_mode = ik_req.SEED_USER if len(seeds) > 0 else ik_req.SEED_CURRENT for seed in seeds: ik_req.seed_angles.append(seed.joint_state) resp = self._kinematics_services['ik']['robot']['service'].call(ik_req) solutions = [] for j, v in zip(resp.joints, resp.isValid): solutions.append(RobotState(is_diff=False, joint_state=j) if v else None) return solutions def _get_ik_trac(self, eef_poses, seeds=(), params=None): ik_req = GetConstrainedPositionIKRequest() if params is None: ik_req.num_steps = 1 else: ik_req.end_tolerance = params['end_tolerance'] ik_req.num_steps = params['num_attempts'] for eef_pose in eef_poses: ik_req.pose_stamp.append(eef_pose) if len(seeds) == 0: seeds = [self.get_current_state()]*len(eef_poses) for seed in seeds: ik_req.seed_angles.append(seed.joint_state) resp = self._kinematics_services['ik']['trac']['service'].call(ik_req) solutions = [] for j, v in zip(resp.joints, resp.isValid): solutions.append(RobotState(is_diff=False, joint_state=j) if v else None) return solutions def _get_ik_ros(self, eef_poses, seeds=()): rqst = GetPositionIKRequest() rqst.ik_request.avoid_collisions = True rqst.ik_request.group_name = self.group_name() solutions = [] for pose_num, eef_pose in enumerate(eef_poses): rqst.ik_request.pose_stamped = eef_pose # Do we really to do a separate call for each pose? _vector not used ik_answer = self._kinematics_services['ik']['ros']['service'].call(rqst) if len(seeds) > 0: rqst.ik_request.robot_state = seeds[pose_num] if ik_answer.error_code.val==1: # Apply a filter to return only joints of this group try: ik_answer.solution.joint_state.velocity = [value for id_joint, value in enumerate(ik_answer.solution.joint_state.velocity) if ik_answer.solution.joint_state.name[id_joint] in self.joint_names()] ik_answer.solution.joint_state.effort = [value for id_joint, value in enumerate(ik_answer.solution.joint_state.effort) if ik_answer.solution.joint_state.name[id_joint] in self.joint_names()] except IndexError: pass ik_answer.solution.joint_state.position = [value for id_joint, value in enumerate(ik_answer.solution.joint_state.position) if ik_answer.solution.joint_state.name[id_joint] in self.joint_names()] ik_answer.solution.joint_state.name = [joint for joint in ik_answer.solution.joint_state.name if joint in self.joint_names()] solutions.append(ik_answer.solution) else: solutions.append(None) return solutions def translate_to_cartesian(self, path, frame_id, time, n_points=50, max_speed=np.pi/4, min_success_rate=0.5, display_only=False, timeout=0, stop_test=lambda:False, pause_test=lambda:False): """ Translate the end effector in straight line, following path=[translate_x, translate_y, translate_z] wrt frame_id :param path: Path [x, y, z] to follow wrt frame_id :param frame_id: frame_id of the given input path :param time: Time of the generated trajectory :param n_points: Number of 3D points of the cartesian trajectory :param max_speed: Maximum speed for every single joint in rad.s-1, allowing to avoid jumps in joints configuration :param min_success_rate: Raise RuntimeError in case the success rate is lower than min_success_rate :param display_only: :param timeout: In case of cuff interaction, indicates the max time to retry before giving up (default is 0 = do not retry) :param stop_test: pointer to a function that returns True if execution must stop now. /!\ Should be fast, it will be called at 100Hz! :param pause_test: pointer to a function that returns True if execution must pause now. If yes it blocks until pause=False again and relaunches the same goal /!\ Test functions must be fast, they will be called at 100Hz! :return: :raises: RuntimeError if success rate is too low """ def trajectory_callable(start): traj, success_rate = trajectories.generate_cartesian_path(path, frame_id, time, self, None, n_points, max_speed) if success_rate < min_success_rate: raise RuntimeError("Unable to generate cartesian path (success rate : {})".format(success_rate)) return traj return self._relaunched_move_to(trajectory_callable, display_only, timeout, stop_test, pause_test) def move_to_controlled(self, goal, rpy=[0, 0, 0], display_only=False, timeout=15, stop_test=lambda:False, pause_test=lambda:False): """ Move to a goal using interpolation in joint space with limitation of velocity and acceleration :param goal: Pose, PoseStamped or RobotState :param rpy: Vector [Roll, Pitch, Yaw] filled with 0 if this axis must be preserved, 1 otherwise :param display_only: :param timeout: In case of cuff interaction, indicates the max time to retry before giving up :param stop_test: pointer to a function that returns True if execution must stop now. /!\ Should be fast, it will be called at 100Hz! :param pause_test: pointer to a function that returns True if execution must pause now. If yes it blocks until pause=False again and relaunches the same goal /!\ Test functions must be fast, they will be called at 100Hz! :return: None :raises: ValueError if IK has no solution """ assert callable(stop_test) assert callable(pause_test) if not isinstance(goal, RobotState): goal = self.get_ik(goal) if goal is None: raise ValueError('This goal is not reachable') # collect the robot state start = self.get_current_state() # correct the orientation if rpy is set if np.array(rpy).any(): # convert the starting point to rpy pose pos, rot = states.state_to_pose(start, self, True) for i in range(3): if rpy[i]: rpy[i] = rot[i] goal = states.correct_state_orientation(goal, rpy, self) # parameters for trapezoidal method kv_max = self.kv_max ka_max = self.ka_max return self._relaunched_move_to(lambda start: trajectories.trapezoidal_speed_trajectory(goal, start=start ,kv_max=kv_max, ka_max=ka_max), display_only, timeout, stop_test, pause_test) def rotate_joint(self, joint_name, goal_angle, display_only=False, stop_test=lambda:False, pause_test=lambda:False): goal = self.get_current_state() joint = goal.joint_state.name.index(joint_name) # JTAS accepts all angles even out of limits #limits = self.joint_limits()[joint_name] goal.joint_state.position[joint] = goal_angle return self.move_to_controlled(goal, display_only=display_only, stop_test=stop_test, pause_test=pause_test) def _relaunched_move_to(self, trajectory_callable, display_only=False, timeout=15, stop_test=lambda:False, pause_test=lambda:False): """ Relaunch several times (until cuff interaction or failure) a move_to() whose trajectory is generated by the callable passed in parameter :param trajectory_callable: Callable to call to recompute the trajectory :param display_only: :param timeout: In case of cuff interaction, indicates the max time to retry before giving up :param stop_test: pointer to a function that returns True if execution must stop now. /!\ Should be fast, it will be called at 100Hz! :param pause_test: pointer to a function that returns True if execution must pause now. If yes it blocks until pause=False again and relaunches the same goal :return: """ assert callable(trajectory_callable) retry = True t0 = rospy.get_time() while retry and rospy.get_time()-t0 < timeout or timeout == 0: start = self.get_current_state() trajectory = trajectory_callable(start) if display_only: self.display(trajectory) break else: retry = not self.execute(trajectory, test=lambda: stop_test() or pause_test()) if pause_test(): if not stop_test(): retry = True while pause_test(): rospy.sleep(0.1) if timeout == 0: return not display_only and not retry if retry: rospy.sleep(1) return not display_only and not retry def get_random_pose(self): # get joint names joint_names = self.joint_names() # create a random joint state bounds = [] for key, value in self.joint_limits().iteritems(): bounds.append(value) bounds = np.array(bounds) joint_state = np.random.uniform(bounds[:, 0], bounds[:, 1], len(joint_names)) # append it in a robot state goal = RobotState() goal.joint_state.name = joint_names goal.joint_state.position = joint_state goal.joint_state.header.stamp = rospy.Time.now() goal.joint_state.header.frame_id = 'base' return goal ######################## OPERATIONS ON TRAJECTORIES # TO BE MOVED IN trajectories.py def interpolate_joint_space(self, goal, total_time, nb_points, start=None): """ Interpolate a trajectory from a start state (or current state) to a goal in joint space :param goal: A RobotState to be used as the goal of the trajectory param total_time: The time to execute the trajectory :param nb_points: Number of joint-space points in the final trajectory :param start: A RobotState to be used as the start state, joint order must be the same as the goal :return: The corresponding RobotTrajectory """ dt = total_time*(1.0/nb_points) # create the joint trajectory message traj_msg = JointTrajectory() rt = RobotTrajectory() if start == None: start = self.get_current_state() joints = [] start_state = start.joint_state.position goal_state = goal.joint_state.position for j in range(len(goal_state)): pose_lin = np.linspace(start_state[j],goal_state[j],nb_points+1) joints.append(pose_lin[1:].tolist()) for i in range(nb_points): point = JointTrajectoryPoint() for j in range(len(goal_state)): point.positions.append(joints[j][i]) # append the time from start of the position point.time_from_start = rospy.Duration.from_sec((i+1)*dt) # append the position to the message traj_msg.points.append(point) # put name of joints to be moved traj_msg.joint_names = self.joint_names() # send the message rt.joint_trajectory = traj_msg return rt def display(self, trajectory): """ Display a joint-space trajectory or a robot state in RVIz loaded with the Moveit plugin :param trajectory: a RobotTrajectory, JointTrajectory, RobotState or JointState message """ # Publish the DisplayTrajectory (only for trajectory preview in RViz) # includes a convert of float durations in rospy.Duration() def js_to_rt(js): rt = RobotTrajectory() rt.joint_trajectory.joint_names = js.name rt.joint_trajectory.points.append(JointTrajectoryPoint(positions=js.position)) return rt dt = DisplayTrajectory() if isinstance(trajectory, RobotTrajectory): dt.trajectory.append(trajectory) elif isinstance(trajectory, JointTrajectory): rt = RobotTrajectory() rt.joint_trajectory = trajectory dt.trajectory.append(rt) elif isinstance(trajectory, RobotState): dt.trajectory.append(js_to_rt(trajectory.joint_state)) elif isinstance(trajectory, JointState): dt.trajectory.append(js_to_rt(trajectory)) else: raise NotImplementedError("ArmCommander.display() expected type RobotTrajectory, JointTrajectory, RobotState or JointState, got {}".format(str(type(trajectory)))) self._display_traj.publish(dt) def execute(self, trajectory, test=None, epsilon=0.1): """ Safely executes a trajectory in joint space on the robot or simulate through RViz and its Moveit plugin (File moveit.rviz must be loaded into RViz) This method is BLOCKING until the command succeeds or failure occurs i.e. the user interacted with the cuff or collision has been detected Non-blocking needs should deal with the JointTrajectory action server :param trajectory: either a RobotTrajectory or a JointTrajectory :param test: pointer to a function that returns True if execution must stop now. /!\ Should be fast, it will be called at 100Hz! :param epsilon: distance threshold on the first point. If distance with first point of the trajectory is greater than espilon execute a controlled trajectory to the first point. Put float(inf) value to bypass this functionality :return: True if execution ended successfully, False otherwise """ def distance_to_first_point(point): joint_pos = np.array(point.positions) return np.linalg.norm(current_array - joint_pos) self.display(trajectory) with self._stop_lock: self._stop_reason = '' if isinstance(trajectory, RobotTrajectory): trajectory = trajectory.joint_trajectory elif not isinstance(trajectory, JointTrajectory): raise TypeError("Execute only accept RobotTrajectory or JointTrajectory") ftg = FollowJointTrajectoryGoal() ftg.trajectory = trajectory # check if it is necessary to move to the first point current = self.get_current_state() current_array = np.array([current.joint_state.position[current.joint_state.name.index(joint)] for joint in trajectory.joint_names]) if distance_to_first_point(trajectory.points[0]) > epsilon: # convert first point to robot state rs = RobotState() rs.joint_state.name = trajectory.joint_names rs.joint_state.position = trajectory.points[0].positions # move to the first point self.move_to_controlled(rs) # execute the input trajectory self.client.send_goal(ftg) # Blocking part, wait for the callback or a collision or a user manipulation to stop the trajectory while self.client.simple_state != SimpleGoalState.DONE: if callable(test) and test(): self.client.cancel_goal() return True if self._stop_reason!='': self.client.cancel_goal() return False self._rate.sleep() return True def close(self): """ Open the gripper :return: True if an object has been grasped """ return self._gripper.close(True) def open(self): """ Close the gripper return: True if an object has been released """ return self._gripper.open(True) def gripping(self): return self._gripper.gripping() def wait_for_human_grasp(self, threshold=1, rate=10, ignore_gripping=True): """ Blocks until external motion is given to the arm :param threshold: :param rate: rate of control loop in Hertz :param ignore_gripping: True if we must wait even if no object is gripped """ def detect_variation(): new_effort = np.array(self.get_current_state([self.name+'_w0', self.name+'_w1', self.name+'_w2']).joint_state.effort) delta = np.absolute(effort - new_effort) return np.amax(delta) > threshold # record the effort at calling time effort = np.array(self.get_current_state([self.name+'_w0', self.name+'_w1', self.name+'_w2']).joint_state.effort) # loop till the detection of changing effort rate = rospy.Rate(rate) while not detect_variation() and not rospy.is_shutdown() and (ignore_gripping or self.gripping()): rate.sleep()
class DobotClient(object): def __init__(self): self.get_ready_srv = rospy.Service("arm/get_ready", Trigger, self.get_ready_cb) self.get_object_type_srv = rospy.ServiceProxy( "/art/db/object_type/get", getObjectType) self.get_pose_srv = rospy.ServiceProxy("/DobotServer/GetPose", GetPose) self.move_to_srv = rospy.ServiceProxy("/DobotServer/SetPTPCmd", SetPTPCmd) self.set_suction_srv = rospy.ServiceProxy( "/DobotServer/SetEndEffectorSuctionCup", SetEndEffectorSuctionCup) self.get_suction_srv = rospy.ServiceProxy( "/DobotServer/GetEndEffectorSuctionCup", GetEndEffectorSuctionCup) self.set_cmd_timeout_srv = rospy.ServiceProxy( "/DobotServer/SetCmdTimeout", SetCmdTimeout) self.set_queued_cmd_clear_srv = rospy.ServiceProxy( "/DobotServer/SetQueuedCmdClear", SetQueuedCmdClear) self.set_queued_cmd_start_exec_srv = rospy.ServiceProxy( "/DobotServer/SetQueuedCmdStartExec", SetQueuedCmdStartExec) self.set_ent_effector_params_srv = rospy.ServiceProxy( "/DobotServer/SetEndEffectorParams", SetEndEffectorParams) self.set_ptp_joint_params_srv = rospy.ServiceProxy( "/DobotServer/SetPTPJointParams", SetPTPJointParams) self.set_ptp_coord_params_srv = rospy.ServiceProxy( "/DobotServer/SetPTPCoordinateParams", SetPTPCoordinateParams) self.set_ptp_jump_params_srv = rospy.ServiceProxy( "/DobotServer/SetPTPJumpParams", SetPTPJumpParams) self.set_ptp_common_params_srv = rospy.ServiceProxy( "/DobotServer/SetPTPCommonParams", SetPTPCommonParams) self._action_name = "arm/pp" self._as = actionlib.SimpleActionServer(self._action_name, PickPlaceAction, self.pick_place_cb, True) self.grasped_object_pub = rospy.Publisher("arm/grasped_object", ObjInstance, queue_size=1, latch=True) self.move_to_sub = rospy.Subscriber("arm/move_to", PoseStamped, self.move_to_cb, queue_size=100) self.object_sub = rospy.Subscriber( "/art/object_detector/object_filtered", InstancesArray, self.objects_cb, queue_size=1) self.tf_broadcaster = TransformBroadcaster() self.tf_listener = TransformListener() self._dobot_pose = Pose() self._objects = InstancesArray() self._grasped_object = ObjInstance() self.grasped_object_pub.publish(self._grasped_object) self._init_dobot() rospy.loginfo("Dobot ready") def pick_place_cb(self, goal): """ Args: goal: @type goal: PickPlaceGoal Returns: """ res = PickPlaceResult() if goal.operation == PickPlaceGoal.RESET: self._objects = None self._grasped_object = None res.result = res.SUCCESS self._as.set_succeeded(res) elif goal.operation == PickPlaceGoal.PICK_OBJECT_ID: res = self.pick_object_id(goal.object) elif goal.operation == PickPlaceGoal.PLACE_TO_POSE: res = self.place_object(goal.pose) elif goal.operation == PickPlaceGoal.GET_READY: self.get_ready() res.result = res.SUCCESS if res.result == res.SUCCESS: self._as.set_succeeded(res) else: self._as.set_aborted(res) def pick_object_id(self, object_id): """ Args: object_id: @type object_id: int Returns: """ for o in self._objects.instances: # type: ObjInstance if o.object_id == object_id: obj = PoseStamped() obj.pose = o.pose obj.header = self._objects.header obj.header.stamp = rospy.Time(0) transformed_obj = self.tf_listener.transformPose( "/base_link", obj) pick_pose = transformed_obj.pose self._grasped_object = deepcopy(o) obj_type = self.get_object_type(o.object_type) # pick_pose.position.z += obj_type.bbox.dimensions[obj_type.bbox.BOX_Z] - 0.005 pick_pose.position.z -= 0.004 pp = PoseStamped() pp.pose = pick_pose self.get_ready_for_pick_place() self.move_to_pose(pp, 0) self.wait_for_final_pose(pp.pose) self.set_suction(True) pp.pose.position.z = max(pp.pose.position.z + 0.05, 0.1) self.move_to_pose(pp, 0) self.wait_for_final_pose(pp.pose) res = PickPlaceResult() res.result = res.SUCCESS self.grasped_object_pub.publish(deepcopy(o)) return res res = PickPlaceResult() res.result = res.FAILURE res.message = "No such object" return res def place_object(self, place_pose): """ Args: place_pose: @type place_pose: PoseStamped Returns: """ print self._grasped_object.object_type print self.get_object_type(self._grasped_object.object_type) obj_type = self.get_object_type(self._grasped_object.object_type) place_pose.pose.position.z = obj_type.bbox.dimensions[ obj_type.bbox.BOX_Z] # self.get_ready_for_pick_place() transformed_pose = self.tf_listener.transformPose( "/base_link", place_pose) try: pp = deepcopy(transformed_pose) pp.pose.position.z = max(pp.pose.position.z + 0.05, 0.15) self.move_to_pose(pp, 1) self.wait_for_final_pose(pp.pose) self.move_to_pose(transformed_pose) self.wait_for_final_pose(transformed_pose.pose) self.set_suction(False) transformed_pose.pose.position.z += 0.03 self.move_to_pose(transformed_pose) self.wait_for_final_pose(transformed_pose.pose) self._grasped_object = ObjInstance() self.grasped_object_pub.publish(self._grasped_object) res = PickPlaceResult() res.result = res.SUCCESS return res except TimeoutException: # failed to place object res = PickPlaceResult() res.result = res.FAILURE res.message = "Failed to place the object" return res def wait_for_final_pose(self, pose, timeout=10): end_time = rospy.Time.now() + rospy.Duration(timeout) r = rospy.Rate(10) while end_time > rospy.Time.now(): if self.poses_max_diff(self._dobot_pose, pose) < 0.02: print("Movement done") return r.sleep() print("Movement failed") raise TimeoutException def poses_max_diff(self, pose1, pose2): """ Args: pose1: pose2: @type pose1: Pose @type pose2: Pose Returns: @rtype: float """ return float( max(abs(pose1.position.x - pose2.position.x), abs(pose1.position.y - pose2.position.y), abs(pose1.position.z - pose2.position.z))) def objects_cb(self, data): """ Args: data: @type data: InstancesArray Returns: """ self._objects = data def _init_dobot(self): cmd_timeout = SetCmdTimeoutRequest() cmd_timeout.timeout = 3000 self.set_cmd_timeout_srv.call(cmd_timeout) self.set_queued_cmd_clear_srv.call(SetQueuedCmdClearRequest()) self.set_queued_cmd_start_exec_srv.call(SetQueuedCmdStartExecRequest()) end_effector_params = SetEndEffectorParamsRequest() end_effector_params.xBias = 70 end_effector_params.yBias = 0 end_effector_params.zBias = 0 self.set_ent_effector_params_srv.call(end_effector_params) ptp_joint_params = SetPTPJointParamsRequest() ptp_joint_params.velocity = ptp_joint_params.acceleration = [ 100, 100, 100, 100 ] self.set_ptp_joint_params_srv.call(ptp_joint_params) ptp_coord_params = SetPTPCoordinateParamsRequest() ptp_coord_params.xyzAcceleration = 100 ptp_coord_params.xyzVelocity = 100 ptp_coord_params.rAcceleration = 100 ptp_coord_params.rVelocity = 100 self.set_ptp_coord_params_srv.call(ptp_coord_params) ptp_jump_params = SetPTPJumpParamsRequest() ptp_jump_params.jumpHeight = 20 ptp_jump_params.zLimit = 200 self.set_ptp_jump_params_srv.call(ptp_jump_params) ptp_common_params = SetPTPCommonParamsRequest() ptp_common_params.velocityRatio = 50 ptp_common_params.accelerationRatio = 50 self.set_ptp_common_params_srv.call(ptp_common_params) self.set_suction(False) def get_pose(self): resp = self.get_pose_srv.call( GetPoseRequest()) # type: GetPoseResponse self._dobot_pose.position.x = resp.x / 1000.0 self._dobot_pose.position.y = resp.y / 1000.0 self._dobot_pose.position.z = resp.z / 1000.0 self.tf_broadcaster.sendTransform( (self._dobot_pose.position.x, self._dobot_pose.position.y, self._dobot_pose.position.z), (0, 0, 0, 1), rospy.Time.now(), "suction_cup", "base_link") def move_to_cb(self, pose): """ Args: pose: @type pose: PoseStamped Returns: """ self.move_to_pose(pose) def move_to_pose(self, pose, mode=1): """ Args: pose: mode: 1 means direct move to pose, 0 means go up, then above desired pose, then down @type pose: PoseStamped @type mode: int Returns: """ print("Move to ") print(pose) req = SetPTPCmdRequest() req.ptpMode = mode req.x = pose.pose.position.x * 1000 # dobot needs position in mm req.y = pose.pose.position.y * 1000 req.z = pose.pose.position.z * 1000 req.r = 0 resp = self.move_to_srv.call(req) # type: SetPTPCmdResponse if resp.result == 0: return True else: return False def get_suction(self): resp = self.get_suction_srv.call(GetEndEffectorSuctionCupRequest( )) # type: GetEndEffectorSuctionCupResponse return resp.suck def set_suction(self, suction): """ Args: suction: @type suction: bool Returns: """ req = SetEndEffectorSuctionCupRequest() req.isQueued = True req.enableCtrl = 1 req.suck = 1 if suction else 0 self.set_suction_srv.call(req) def get_object_type(self, object_type): """ Args: object_type: @type object_type: str Returns: getObjectTypeResponse """ req = getObjectTypeRequest() req.name = object_type resp = self.get_object_type_srv.call( req) # type: getObjectTypeResponse return resp.object_type def get_ready_cb(self, req): self.get_ready() resp = TriggerResponse() resp.success = True return resp def get_ready(self): pose = PoseStamped() pose.pose.position.x = 0.17 pose.pose.position.y = 0.0 pose.pose.position.z = 0.03 self.move_to_pose(pose) self.wait_for_final_pose(pose.pose) def get_ready_for_pick_place(self): pose = PoseStamped() pose.pose.position.x = 0.25 pose.pose.position.z = 0.1 self.move_to_pose(pose) self.wait_for_final_pose(pose.pose)
class Controller(): Manual = 0 Automatic = 1 TakeOff = 2 def __init__(self): self.pubNav = rospy.Publisher('cmd_vel', Twist, queue_size=1) self.listener = TransformListener() rospy.Subscriber("joy", Joy, self._joyChanged) rospy.Subscriber("cmd_vel_telop", Twist, self._cmdVelTelopChanged) self.cmd_vel_telop = Twist() self.pidX = PID(20, 12, 0.0, -30, 30, "x") self.pidY = PID(-20, -12, 0.0, -30, 30, "y") self.pidZ = PID(15000, 3000.0, 1500.0, 10000, 60000, "z") self.pidYaw = PID(-50.0, 0.0, 0.0, -200.0, 200.0, "yaw") self.state = Controller.Manual self.targetZ = 0.5 self.lastJoy = Joy() def _cmdVelTelopChanged(self, data): self.cmd_vel_telop = data if self.state == Controller.Manual: self.pubNav.publish(data) def pidReset(self): self.pidX.reset() self.pidZ.reset() self.pidZ.reset() self.pidYaw.reset() def _joyChanged(self, data): if len(data.buttons) == len(self.lastJoy.buttons): delta = np.array(data.buttons) - np.array(self.lastJoy.buttons) print ("Buton ok") #Button 1 if delta[0] == 1 and self.state != Controller.Automatic: print("Automatic!") thrust = self.cmd_vel_telop.linear.z print(thrust) self.pidReset() self.pidZ.integral = thrust / self.pidZ.ki self.targetZ = 0.5 self.state = Controller.Automatic #Button 2 if delta[1] == 1 and self.state != Controller.Manual: print("Manual!") self.pubNav.publish(self.cmd_vel_telop) self.state = Controller.Manual #Button 3 if delta[2] == 1: self.state = Controller.TakeOff print("TakeOff!") #Button 5 if delta[4] == 1: self.targetZ += 0.1 print(self.targetZ) #Button 6 if delta[5] == 1: self.targetZ -= 0.1 print(self.targetZ) self.lastJoy = data def run(self): thrust = 0 print("jello") while not rospy.is_shutdown(): if self.state == Controller.TakeOff: t = self.listener.getLatestCommonTime("/mocap", "/Nano_Mark") if self.listener.canTransform("/mocap", "/Nano_Mark", t): position, quaternion = self.listener.lookupTransform("/mocap", "/Nano_Mark", t) if position[2] > 0.1 or thrust > 50000: self.pidReset() self.pidZ.integral = thrust / self.pidZ.ki self.targetZ = 0.5 self.state = Controller.Automatic thrust = 0 else: thrust += 100 msg = self.cmd_vel_telop msg.linear.z = thrust self.pubNav.publish(msg) if self.state == Controller.Automatic: # transform target world coordinates into local coordinates t = self.listener.getLatestCommonTime("/Nano_Mark", "/mocap") print(t); if self.listener.canTransform("/Nano_Mark", "/mocap", t): targetWorld = PoseStamped() targetWorld.header.stamp = t targetWorld.header.frame_id = "mocap" targetWorld.pose.position.x = 0 targetWorld.pose.position.y = 0 targetWorld.pose.position.z = self.targetZ quaternion = tf.transformations.quaternion_from_euler(0, 0, 0) targetWorld.pose.orientation.x = quaternion[0] targetWorld.pose.orientation.y = quaternion[1] targetWorld.pose.orientation.z = quaternion[2] targetWorld.pose.orientation.w = quaternion[3] targetDrone = self.listener.transformPose("/Nano_Mark", targetWorld) quaternion = ( targetDrone.pose.orientation.x, targetDrone.pose.orientation.y, targetDrone.pose.orientation.z, targetDrone.pose.orientation.w) euler = tf.transformations.euler_from_quaternion(quaternion) #msg = self.cmd_vel_teleop msg.linear.x = self.pidX.update(0.0, targetDrone.pose.position.x) msg.linear.y = self.pidY.update(0.0, targetDrone.pose.position.y) msg.linear.z = self.pidZ.update(0.0, targetDrone.pose.position.z) #self.pidZ.update(position[2], self.targetZ) msg.angular.z = self.pidYaw.update(0.0, euler[2]) #print(euler[2]) #print(msg.angular.z) self.pubNav.publish(msg) rospy.sleep(0.01)
class ArmActions(): def __init__(self): rospy.init_node('left_arm_actions') self.tfl = TransformListener() self.aul = l_arm_utils.ArmUtils(self.tfl) #self.fth = ft_handler.FTHandler() rospy.loginfo("Waiting for l_utility_frame_service") try: rospy.wait_for_service('/l_utility_frame_update', 7.0) self.aul.update_frame = rospy.ServiceProxy( '/l_utility_frame_update', FrameUpdate) rospy.loginfo("Found l_utility_frame_service") except: rospy.logwarn("Left Utility Frame Service Not available") rospy.loginfo('Waiting for Pixel_2_3d Service') try: rospy.wait_for_service('/pixel_2_3d', 7.0) self.p23d_proxy = rospy.ServiceProxy('/pixel_2_3d', Pixel23d, True) rospy.loginfo("Found pixel_2_3d service") except: rospy.logwarn("Pixel_2_3d Service Not available") #Low-level motion requests: pass directly to arm_utils rospy.Subscriber('wt_left_arm_pose_commands', Point, self.torso_frame_move) rospy.Subscriber('wt_left_arm_angle_commands', JointTrajectoryPoint, self.aul.send_traj_point) #More complex motion scripts, defined here using arm_util functions rospy.Subscriber('norm_approach_left', PoseStamped, self.norm_approach) rospy.Subscriber('wt_grasp_left_goal', PoseStamped, self.grasp) #rospy.Subscriber('wt_wipe_left_goals', PoseStamped, self.wipe) rospy.Subscriber('wt_wipe_left_goals', PoseStamped, self.force_wipe_agg) rospy.Subscriber('wt_swipe_left_goals', PoseStamped, self.swipe) rospy.Subscriber('wt_lin_move_left', Float32, self.hand_move) rospy.Subscriber('wt_adjust_elbow_left', Float32, self.aul.adjust_elbow) rospy.Subscriber('wt_surf_wipe_left_points', Point, self.force_surf_wipe) rospy.Subscriber('wt_poke_left_point', PoseStamped, self.poke) rospy.Subscriber('wt_contact_approach_left', PoseStamped, self.approach_to_contact) self.wt_log_out = rospy.Publisher('wt_log_out', String) self.test_pose = rospy.Publisher('test_pose', PoseStamped, latch=True) self.say = rospy.Publisher('text_to_say', String) self.wipe_started = False self.surf_wipe_started = False self.wipe_ends = [PoseStamped(), PoseStamped()] #FORCE_TORQUE ADDITIONS #rospy.Subscriber('pr2_netft_zeroer/wrench_zeroed', WrenchStamped, self.ft_preprocess) #self.rezero_wrench = rospy.Publisher('pr2_netft_zeroer/rezero_wrench', Bool) rospy.Subscriber('ft_data_pm_adjusted', WrenchStamped, self.ft_preprocess) rospy.Subscriber('wt_ft_goal', Float32, self.set_force_goal) self.wt_force_out = rospy.Publisher('wt_force_out', Float32) self.ft_rate_limit = rospy.Rate(30) self.ft = WrenchStamped() self.ft_mag = 0. self.ft_mag_que = deque([0] * 10, 10) self.ft_sm_mag = 0. self.ft_case = None self.force_goal_mean = 3 #1.42 self.force_goal_std = 0.625 self.stop_maintain = False self.force_wipe_started = False self.force_wipe_start = PoseStamped() self.force_wipe_appr = PoseStamped() def set_force_goal(self, msg): self.force_goal_mean = msg.data def ft_preprocess(self, ft): self.ft = ft self.ft_mag = math.sqrt(ft.wrench.force.x**2 + ft.wrench.force.y**2 + ft.wrench.force.z**2) self.ft_mag_que.append(self.ft_mag) self.ft_sm_mag = np.mean(self.ft_mag_que) #print 'Force Magnitude: ', self.ft_mag self.wt_force_out.publish(self.ft_mag) def approach_to_contact(self, ps, overshoot=0.05): ps.pose.position.z += 0.02 self.stop_maintain = True self.aul.update_frame(ps) appr = self.aul.find_approach(ps, 0.15) goal = self.aul.find_approach(ps, -overshoot) (appr_reachable, ik_goal) = self.aul.full_ik_check(appr) (goal_reachable, ik_goal) = self.aul.full_ik_check(goal) if appr_reachable and goal_reachable: traj = self.aul.build_trajectory(goal, appr, tot_points=200) #prep = self.aul.move_arm_to(appr) self.aul.blind_move(appr, 3) rospy.sleep(3) if True: # if prep: self.adjust_forearm(traj.points[0].positions) #self.rezero_wrench.publish(data=True) curr_traj_point = self.advance_to_contact(traj) if not curr_traj_point is None: self.maintain_norm_force2(traj, curr_traj_point) #self.maintain_force_position(self.aul.hand_frame_move(0.05)) #self.twist_wipe(); self.aul.blind_move(appr) else: self.aul.send_traj_point(traj.points[0], 4) else: rospy.loginfo("Cannot reach desired 'move-to-contact' point") self.wt_log_out.publish( data="Cannot reach desired 'move-to-contact' point") def advance_to_contact(self, traj): self.stop_maintain = False curr_traj_point = 0 advance_rate = rospy.Rate(10) while not (rospy.is_shutdown() or self.stop_maintain): if not (curr_traj_point >= len(traj.points)): self.aul.send_traj_point(traj.points[curr_traj_point], 0.3) curr_traj_point += 1 advance_rate.sleep() else: rospy.loginfo( "Moved past expected contact, but no contact found! Returning to start" ) self.wt_log_out.publish( data= "Moved past expected contact, but no contact found! Returning to start" ) self.stop_maintain = True return None if self.ft_mag > 2.5: self.stop_maintain = True print "Contact Detected" return curr_traj_point def maintain_norm_force2(self, traj, curr_traj_point=0, mean=3, std=1): self.stop_maintain = False maintain_rate = rospy.Rate(100) while not (rospy.is_shutdown() or self.stop_maintain): if self.ft_mag > 12: rospy.loginfo("Force Too High, ending behavior") self.wt_log_out.publish(data="Force too high, ending behavior") break #print "mean: ", mean, "stds: ", std, "force: ", self.ft_mag if self.ft_mag < mean - std: curr_traj_point += 1 curr_traj_point = np.clip(curr_traj_point, 0, len(traj.points)) #print curr_traj_point print "Low" if not (curr_traj_point >= len(traj.points)): self.aul.send_traj_point(traj.points[curr_traj_point], 0.3) rospy.sleep(0.3) #else: # rospy.loginfo("Force too low, but extent of the trajectory is reached") # self.wt_log_out.publish(data="Force too low, but extent of the trajectory is reached") # self.stop_maintain = True elif self.ft_mag > mean + std: print "High" steps = int(round((self.ft_mag / std))) curr_traj_point -= steps #print curr_traj_point curr_traj_point = np.clip(curr_traj_point, 0, len(traj.points)) if curr_traj_point >= 0: self.aul.send_traj_point(traj.points[curr_traj_point], 0.3) rospy.sleep(0.3) else: rospy.loginfo( "Beginning of trajectory reached, cannot back away further" ) self.wt_log_out.publish( data= "Beginning of trajectory reached, cannot back away further" ) #self.stop_maintain = True maintain_rate.sleep() mean = self.force_goal_mean std = self.force_goal_std print "Returning to start position" self.aul.send_traj_point(traj.points[0], 4) def maintain_norm_force(self, traj, curr_traj_point=0, mean=0, std=1): self.stop_maintain = False maintain_rate = rospy.Rate(100) while not (rospy.is_shutdown() or self.stop_maintain): #print "mean: ", mean, "stds: ", std, "force: ", self.ft_mag if self.ft_mag < mean - std: curr_traj_point += 1 np.clip(curr_traj_point, 0, len(traj.points)) if not (curr_traj_point >= len(traj.points)): print curr_traj_point self.aul.send_traj_point(traj.points[curr_traj_point], 0.1) rospy.sleep(0.1) else: rospy.loginfo( "Force too low, but extent of the trajectory is reached" ) self.wt_log_out.publish( data= "Force too low, but extent of the trajectory is reached" ) stopped = True elif self.ft_mag > mean + std: curr_traj_point -= 1 np.clip(curr_traj_point, 0, len(traj.points)) if curr_traj_point >= 0: print curr_traj_point self.aul.send_traj_point(traj.points[curr_traj_point], 0.1) rospy.sleep(0.1) else: rospy.loginfo( "Beginning of trajectory reached, cannot back away further" ) self.wt_log_out.publish( data= "Beginning of trajectory reached, cannot back away further" ) stopped = True maintain_rate.sleep() mean = self.force_goal_mean std = self.force_goal_std # def maintain_net_force(self, mean=0, std=3): # self.stop_maintain = False # maintain_rate = rospy.Rate(100) # while not (rospy.is_shutdown() or self.stop_maintain): # if self.ft_mag > mean + 8: # curr_angs = self.aul.joint_state_act.positions # try: # x_plus = np.subtract(self.aul.ik_pose_proxy(self.aul.form_ik_request(self.aul.hand_frame_move(0.02))).solution.joint_state.position, curr_angs) # x_minus = np.subtract(self.aul.ik_pose_proxy(self.aul.form_ik_request(self.aul.hand_frame_move(-0.02))).solution.joint_state.position, curr_angs) # y_plus = np.subtract(self.aul.ik_pose_proxy(self.aul.form_ik_request(self.aul.hand_frame_move(0, 0.02, 0))).solution.joint_state.position, curr_angs) # y_minus = np.subtract(self.aul.ik_pose_proxy(self.aul.form_ik_request(self.aul.hand_frame_move(0, -0.02, 0))).solution.joint_state.position, curr_angs) # z_plus = np.subtract(self.aul.ik_pose_proxy(self.aul.form_ik_request(self.aul.hand_frame_move(0, 0, 0.02))).solution.joint_state.position, curr_angs) # z_minus = np.subtract(self.aul.ik_pose_proxy(self.aul.form_ik_request(self.aul.hand_frame_move(0, 0, -0.02))).solution.joint_state.position, curr_angs) # #print 'x: ', x_plus,'\r\n', x_minus # #print 'y: ', y_plus,'\r\n', y_minus # #print 'z: ', z_plus,'\r\n', z_minus # ft_sum = self.ft_mag # parts = np.divide([self.ft.wrench.force.x, self.ft.wrench.force.y, self.ft.wrench.force.z], ft_sum) # print 'parts', parts # ends = [[x_plus,x_minus],[y_plus, y_minus],[z_plus,z_minus]] # side = [[0]*7]*3 # for i, part in enumerate(parts): # if part >=0: # side[i] = ends[i][0] # else: # side[i] = ends[i][1] # # ang_out = curr_angs # for i in range(3): # ang_out -= np.average(side, 0, parts) # except: # print 'Near Joint Limits!' # self.aul.send_joint_angles(ang_out) # # #print 'x: ', x_plus, x_minus # maintain_rate.sleep() # mean = self.force_goal_mean # std = self.force_goal_std def maintain_force_position(self, pose=None, mean=3, std=1): self.stop_maintain = False if pose is None: goal = self.aul.curr_pose() else: goal = pose goal_ort = [ goal.pose.orientation.x, goal.pose.orientation.y, goal.pose.orientation.z, goal.pose.orientation.w ] error = PoseStamped() maintain_rate = rospy.Rate(250) while not (rospy.is_shutdown() or self.stop_maintain): current = self.aul.curr_pose() current_ort = [ current.pose.orientation.x, current.pose.orientation.y, current.pose.orientation.z, current.pose.orientation.w ] error.pose.position.x = current.pose.position.x - goal.pose.position.x error.pose.position.y = current.pose.position.y - goal.pose.position.y error.pose.position.z = current.pose.position.z - goal.pose.position.z error_mag = math.sqrt(error.pose.position.x**2 + error.pose.position.y**2 + error.pose.position.z**2) #out = deepcopy(goal) out = PoseStamped() out.header.frame_id = goal.header.frame_id out.header.stamp = rospy.Time.now() out.pose.position = Point(goal.pose.position.x, goal.pose.position.y, goal.pose.position.z) self.test_pose.publish(out) if all(np.array(self.ft_mag_que) < mean - std) and error_mag > 0.005: #print 'Force Too LOW' out.pose.position.x += 0.990 * error.pose.position.x out.pose.position.y += 0.990 * error.pose.position.y out.pose.position.z += 0.990 * error.pose.position.z ori = transformations.quaternion_slerp(goal_ort, current_ort, 0.990) out.pose.orientation = Quaternion(*ori) self.aul.fast_move(out, 0.0038) elif all(np.array(self.ft_mag_que) > mean + std): #print 'Moving to avoid force' current.pose.position.x += self.ft.wrench.force.x / 9000 current.pose.position.y += self.ft.wrench.force.y / 9000 current.pose.position.z += self.ft.wrench.force.z / 9000 self.aul.fast_move(current, 0.0038) else: pass #print "Force in desired range" mean = self.force_goal_mean std = self.force_goal_std #rospy.sleep(0.001) maintain_rate.sleep() #def maintain_force_position(self,pose = None, mean=3, std=1): # self.stop_maintain = False # if pose is None: # goal = self.aul.curr_pose() # else: # goal = pose # self.rezero_wrench.publish(data=True) # maintain_rate = rospy.Rate(500) # kp = 0.07 # kd = 0.03 # ki = 0.0 # error = PoseStamped() # old_error = PoseStamped() # sum_error_x = deque([0]*10,10) # sum_error_y = deque([0]*10,10) # sum_error_z = deque([0]*10,10) # while not (rospy.is_shutdown() or self.stop_maintain): # current = self.aul.curr_pose() # error.pose.position.x = current.pose.position.x - goal.pose.position.x # error.pose.position.y = current.pose.position.y - goal.pose.position.y # error.pose.position.z = current.pose.position.z - goal.pose.position.z # sum_error_x.append(error.pose.position.x) # sum_error_y.append(error.pose.position.y) # sum_error_z.append(error.pose.position.z) # print "Force: ", self.ft_sm_mag, min(self.ft_mag_que), max(self.ft_mag_que) # print "Error: ", error.pose.position # print self.ft_mag_que, mean-std # print self.ft_mag_que < mean-std # break # if all([self.ft_mag_que < mean - std]): # print 'Force Too LOW' # current.pose.position.x += kp*error.pose.position.x + kd*(error.pose.position.x - old_error.pose.position.x) + ki*np.sum(sum_error_x) # current.pose.position.x += kp*error.pose.position.y + kd*(error.pose.position.y - old_error.pose.position.y) + ki*np.sum(sum_error_y) # current.pose.position.x += kp*error.pose.position.z + kd*(error.pose.position.z - old_error.pose.position.z) + ki*np.sum(sum_error_z) # self.aul.fast_move(current, 0.02) # self.test_pose.publish(current) # if all([i > mean + std for i in self.ft_mag_que]): # print 'Moving to avoid force' # current.pose.position.x += self.ft.wrench.force.x/10000 # current.pose.position.y += self.ft.wrench.force.y/10000 # current.pose.position.z += self.ft.wrench.force.z/10000 # self.aul.fast_move(current, 0.02) # self.test_pose.publish(current) # old_error = error # mean = self.force_goal_mean # std = self.force_goal_std # maintain_rate.sleep() def maintain_net_force(self, mean=3, std=1): self.stop_maintain = False maintain_rate = rospy.Rate(500) #self.rezero_wrench.publish(data=True) while not (rospy.is_shutdown() or self.stop_maintain): if self.ft_mag > mean + 3: print 'Moving to avoid force' print "Force: ", self.ft_mag goal = self.aul.curr_pose() goal.pose.position.x += np.clip(self.ft.wrench.force.x / 5000, -0.001, 0.001) goal.pose.position.y += np.clip(self.ft.wrench.force.y / 5000, -0.001, 0.001) goal.pose.position.z += np.clip(self.ft.wrench.force.z / 5000, -0.001, 0.001) self.test_pose.publish(goal) self.aul.fast_move(goal, 0.02) mean = self.force_goal_mean std = self.force_goal_std maintain_rate.sleep() def mannequin(self): mannequin_rate = rospy.Rate(100) pose = PoseStamped() while not rospy.is_shutdown(): #joints = np.add(np.array(self.aul.joint_state_act.positions), np.clip(np.array(self.aul.joint_state_err.positions), -0.05, 0.05)) joints = self.aul.joint_state_act.positions print joints #raw_input('Review Joint Angles') self.aul.send_joint_angles(joints, 0.00001) pose.header.stamp = rospy.Time.now() self.test_pose.publish(pose) mannequin_rate.sleep() def force_wipe_agg(self, ps): ps.pose.position.z += 0.02 self.aul.update_frame(ps) rospy.sleep(0.1) pose = self.aul.find_approach(ps, 0) (goal_reachable, ik_goal) = self.aul.ik_check(pose) if goal_reachable: if not self.force_wipe_started: appr = self.aul.find_approach(ps, 0.20) (appr_reachable, ik_goal) = self.aul.ik_check(appr) self.test_pose.publish(appr) if appr_reachable: self.force_wipe_start = pose self.force_wipe_appr = appr self.force_wipe_started = True else: rospy.loginfo( "Cannot reach approach point, please choose another") self.wt_log_out.publish( data= "Cannot reach approach point, please choose another") self.say.publish( data= "I cannot get to a safe approach for there, please choose another point" ) else: ps1, ps2 = self.align_poses(self.force_wipe_start, pose) self.force_wipe_prep(ps1, ps2) # self.force_wipe_prep(self.force_wipe_start, pose) self.force_wipe_started = False else: rospy.loginfo("Cannot reach wiping point, please choose another") self.wt_log_out.publish( data="Cannot reach wiping point, please choose another") self.say.publish( data="I cannot reach there, please choose another point") def force_wipe_prep(self, ps_start, ps_finish, travel=0.05): ps_start.header.stamp = rospy.Time.now() ps_finish.header.stamp = rospy.Time.now() ps_start_far = self.aul.hand_frame_move(travel, 0, 0, ps_start) ps_start_near = self.aul.hand_frame_move(-travel, 0, 0, ps_start) ps_finish_far = self.aul.hand_frame_move(travel, 0, 0, ps_finish) ps_finish_near = self.aul.hand_frame_move(-travel, 0, 0, ps_finish) n_points = int( math.ceil(self.aul.calc_dist(ps_start, ps_finish) * 9000)) print 'n_points: ', n_points mid_traj = self.aul.build_trajectory(ps_finish, ps_start, tot_points=n_points, jagged=False) near_traj = self.aul.build_trajectory(ps_finish_near, ps_start_near, tot_points=n_points, jagged=False) far_traj = self.aul.build_trajectory(ps_finish_far, ps_start_far, tot_points=n_points, jagged=False) self.force_wipe(mid_traj, near_traj, far_traj) def force_surf_wipe(self, point): self.fsw_poses = self.prep_surf_wipe(point) if not self.fsw_poses is None: near_poses = far_poses = [ PoseStamped() for i in xrange(len(self.fsw_poses)) ] for i, p in enumerate(self.fsw_poses): near_poses[i] = self.aul.pose_frame_move(p, -0.05) far_poses[i] = self.aul.pose_frame_move(p, 0.05) near_traj = self.aul.fill_ik_traj(near_poses) mid_traj = self.aul.fill_ik_traj(self.fsw_poses) far_traj = self.aul.fill_ik_traj(far_poses) print 'Trajectories Found' self.force_wipe(mid_traj, near_traj, far_traj) def adjust_forearm(self, in_angles): print 'cur angles: ', self.aul.joint_state_act.positions, 'angs: ', in_angles print np.abs(np.subtract(self.aul.joint_state_act.positions, in_angles)) if np.max( np.abs( np.subtract(self.aul.joint_state_act.positions, in_angles))) > math.pi: self.say.publish(data="Adjusting for-arm roll") print "Evasive Action!" angles = list(self.aul.joint_state_act.positions) flex = in_angles[5] angles[5] = -0.1 self.aul.send_joint_angles(angles, 4) angles[4] = in_angles[4] self.aul.send_joint_angles(angles, 6) angles[5] = flex self.aul.send_joint_angles(angles, 4) def force_wipe(self, mid_traj, near_traj, far_traj): near_angles = [ list(near_traj.points[i].positions) for i in range(len(near_traj.points)) ] mid_angles = [ list(mid_traj.points[i].positions) for i in range(len(mid_traj.points)) ] far_angles = [ list(far_traj.points[i].positions) for i in range(len(far_traj.points)) ] print 'lens: nmf: ', len(near_angles), len(mid_angles), len(far_angles) fmn_diff = np.abs(np.subtract(near_angles, far_angles)) fmn_max = np.max(fmn_diff, axis=0) print 'fmn_max: ', fmn_max if any(fmn_max > math.pi / 2): rospy.loginfo( "TOO LARGE OF AN ANGLE CHANGE ALONG GRADIENT, IK REGION PROBLEMS!" ) self.wt_log_out.publish( data= "The path requested required large movements (IK Limits cause angle wrapping)" ) self.say.publish( data="Large motion detected, cancelling. Please try again.") return for i in range(7): n_max = max(np.abs(np.diff(near_angles, axis=0)[i])) m_max = max(np.abs(np.diff(mid_angles, axis=0)[i])) f_max = max(np.abs(np.diff(far_angles, axis=0)[i])) n_mean = 4 * np.mean(np.abs(np.diff(near_angles, axis=0)[i])) m_mean = 4 * np.mean(np.abs(np.diff(mid_angles, axis=0)[i])) f_mean = 4 * np.mean(np.abs(np.diff(far_angles, axis=0)[i])) print 'near: max: ', n_max, 'mean: ', n_mean print 'mid: max: ', m_max, 'mean: ', m_mean print 'far: max: ', f_max, 'mean: ', f_mean if (n_max >= n_mean) or (m_max >= m_mean) or (f_max >= f_mean): rospy.logerr( "TOO LARGE OF AN ANGLE CHANGE ALONG PATH, IK REGION PROBLEMS!" ) self.wt_log_out.publish( data= "The path requested required large movements (IK Limits cause angle wrapping)" ) self.say.publish( data="Large motion detected, cancelling. Please try again." ) return near_diff = np.subtract(near_angles, mid_angles).tolist() far_diff = np.subtract(far_angles, mid_angles).tolist() self.say.publish(data="Moving to approach point") appr = self.force_wipe_appr appr.pose.orientation = self.aul.get_fk( near_angles[0]).pose.orientation #prep = self.aul.move_arm_to(appr) self.aul.blind_move(appr) #rospy.sleep(3) if True: #prep: self.adjust_forearm(near_angles[0]) self.say.publish(data="Making Approach.") bias = 2 self.aul.send_joint_angles( np.add(mid_angles[0], np.multiply(bias, near_diff[0])), 3.5) #self.rezero_wrench.publish(data=True) rospy.sleep(1) wipe_rate = rospy.Rate(1000) self.stop_maintain = False count = 0 lap = 0 max_laps = 4 mean = self.force_goal_mean std = self.force_goal_std self.say.publish(data="Wiping") single_dir = False #True time = rospy.Time.now().to_sec() while not (rospy.is_shutdown() or self.stop_maintain) and ( count + 1 <= len(mid_angles)) and (lap < max_laps): if self.ft_mag > 10: angles_out = np.add(mid_angles[0], np.multiply(2, near_diff[0])) self.aul.send_joint_angles(angles_out, 2) rospy.loginfo("Force Too High, ending behavior") self.wt_log_out.publish( data="Force too high, ending behavior") break #print "mean: ", mean, "std: ", std, "force: ", self.ft_mag if self.ft_mag >= mean + std: # print 'Force too high!' bias += (self.ft_mag / 500) elif self.ft_mag < mean - std: # print 'Force too low' bias -= max(0.003, (self.ft_mag / 1500)) else: # print 'Force Within Range' count += 1 bias = np.clip(bias, -1, 2) if bias > 0.: diff = near_diff[count] else: diff = far_diff[count] angles_out = np.add(mid_angles[count], np.multiply(abs(bias), diff)) self.aul.send_joint_angles(angles_out, 0.0025) #rospy.sleep(0.0000i1) #print "Rate: ", (1/ (rospy.Time.now().to_sec() - time)) #time = rospy.Time.now().to_sec() wipe_rate.sleep() mean = self.force_goal_mean std = self.force_goal_std if count + 1 >= len(mid_angles): if single_dir: bias = 1 pose = self.aul.curr_pose() pose = self.aul.hand_frame_move(-0.01) rot = transformations.quaternion_about_axis( math.radians(-10), (0, 1, 0)) q = transformations.quaternion_multiply([ pose.pose.orientation.x, pose.pose.orientation.y, pose.pose.orientation.z, pose.pose.orientation.w ], rot) pose.pose.orientation = Quaternion(*q) self.aul.blind_move(pose) #goal = self.aul.ik_pose_proxy(self.aul.form_ik_request(pose)) #if goal.error_code.val == 1: # self.aul.send_angles_wrap(goal.solution.joint_state.position) #angles_out = list(self.aul.joint_state_act.positions) #angles_out[4] += 0.05 # self.aul.send_joint_angles(angles_out,3) angles_out = np.add( mid_angles[count], np.multiply(bias, near_diff[count])) self.aul.send_joint_angles(angles_out, 5) angles_out = np.add(mid_angles[0], np.multiply(bias, near_diff[0])) self.aul.send_joint_angles(angles_out, 5) else: mid_angles.reverse() near_diff.reverse() far_diff.reverse() lap += 1 #if lap == 3: # self.say.publish(data="Hold Still, you rascal!") count = 0 rospy.sleep(0.5) self.say.publish(data="Pulling away") angles_out = np.add(mid_angles[0], np.multiply(2, near_diff[0])) self.aul.send_joint_angles(angles_out, 5) rospy.sleep(5) self.say.publish(data="Finished wiping. Thank you") def torso_frame_move(self, msg): self.stop_maintain = True goal = self.aul.curr_pose() goal.pose.position.x += msg.x goal.pose.position.y += msg.y goal.pose.position.z += msg.z self.aul.blind_move(goal) def hand_move(self, f32): self.stop_maintain = True hfm_pose = self.aul.hand_frame_move(f32.data) self.aul.blind_move(hfm_pose) def norm_approach(self, pose): self.stop_maintain = True self.aul.update_frame(pose) appr = self.aul.find_approach(pose, 0.15) self.aul.move_arm_to(appr) def grasp(self, ps): self.stop_maintain = True rospy.loginfo("Initiating Grasp Sequence") self.wt_log_out.publish(data="Initiating Grasp Sequence") self.aul.update_frame(ps) approach = self.aul.find_approach(ps, 0.15) rospy.loginfo("approach: \r\n %s" % approach) at_appr = self.aul.move_arm_to(approach) rospy.loginfo("arrived at approach: %s" % at_appr) if at_appr: opened = self.aul.gripper(0.09) if opened: rospy.loginfo("making linear approach") #hfm_pose = self.aul.hand_frame_move(0.23) hfm_pose = self.aul.find_approach(ps, -0.02) self.aul.blind_move(hfm_pose) self.aul.wait_for_stop(2) closed = self.aul.gripper(0.0) if not closed: rospy.loginfo( "Couldn't close completely: Grasp likely successful") hfm_pose = self.aul.hand_frame_move(-0.23) self.aul.blind_move(hfm_pose) else: pass def prep_surf_wipe(self, point): pixel_u = point.x pixel_v = point.y test_pose = self.p23d_proxy(pixel_u, pixel_v).pixel3d self.aul.update_frame(test_pose) test_pose = self.aul.find_approach(test_pose, 0) (reachable, ik_goal) = self.aul.full_ik_check(test_pose) if reachable: if not self.surf_wipe_started: start_pose = test_pose self.surf_wipe_start = [pixel_u, pixel_v, start_pose] self.surf_wipe_started = True rospy.loginfo( "Received valid starting position for wiping action") self.wt_log_out.publish( data="Received valid starting position for wiping action") return None #Return after 1st point, wait for second else: rospy.loginfo( "Received valid ending position for wiping action") self.wt_log_out.publish( data="Received valid ending position for wiping action") self.surf_wipe_started = False #Continue on successful 2nd point else: rospy.loginfo("Cannot reach wipe position, please try another") self.wt_log_out.publish( data="Cannot reach wipe position, please try another") return None #Return on invalid point, wait for another dist = self.aul.calc_dist(self.surf_wipe_start[2], test_pose) print 'dist', dist num_points = dist / 0.01 print 'num_points', num_points us = np.round(np.linspace(self.surf_wipe_start[0], pixel_u, num_points)) vs = np.round(np.linspace(self.surf_wipe_start[1], pixel_v, num_points)) surf_points = [PoseStamped() for i in xrange(len(us))] print "Surface Points", [us, vs] for i in xrange(len(us)): pose = self.p23d_proxy(us[i], vs[i]).pixel3d self.aul.update_frame(pose) surf_points[i] = self.aul.find_approach(pose, 0) print i + 1, '/', len(us) return surf_points #self.aul.blind_move(surf_points[0]) #self.aul.wait_for_stop() #for pose in surf_points: # self.aul.blind_move(pose,2.5) # rospy.sleep(2) # #self.aul.wait_for_stop() #self.aul.hand_frame_move(-0.1) def twist_wipe(self): angles = list(self.aul.joint_state_act.positions) count = 0 while count < 3: angles[6] = -6.7 self.aul.send_joint_angles(angles) while self.aul.joint_state_act.positions[6] > -6.6: rospy.sleep(0.1) angles[6] = 0.8 self.aul.send_joint_angles(angles) while self.aul.joint_state_act.positions[6] < 0.7: rospy.sleep(0.1) count += 1 def poke(self, ps): self.stop_maintain = True self.aul.update_frame(ps) appr = self.aul.find_approach(ps, 0.15) touch = self.aul.find_approach(ps, 0) prepared = self.aul.move_arm_to(appr) if prepared: self.aul.blind_move(touch) self.aul.wait_for_stop() rospy.sleep(7) self.aul.blind_move(appr) def swipe(self, ps): traj = self.prep_wipe(ps) if traj is not None: self.stop_maintain = True self.wipe_move(traj, 1) def wipe(self, ps): traj = self.prep_wipe(ps) if traj is not None: self.stop_maintain = True self.wipe_move(traj, 4) def prep_wipe(self, ps): #print "Prep Wipe Received: %s" %pa self.aul.update_frame(ps) print "Updating frame to: %s \r\n" % ps if not self.wipe_started: self.wipe_appr_seed = ps self.wipe_ends[0] = self.aul.find_approach(ps, 0) print "wipe_end[0]: %s" % self.wipe_ends[0] (reachable, ik_goal) = self.aul.full_ik_check(self.wipe_ends[0]) if not reachable: rospy.loginfo( "Cannot find approach for initial wipe position, please try another" ) self.wt_log_out.publish( data= "Cannot find approach for initial wipe position, please try another" ) return None else: self.wipe_started = True rospy.loginfo("Received starting position for wiping action") self.wt_log_out.publish( data="Received starting position for wiping action") return None else: self.wipe_ends[1] = self.aul.find_approach(ps, 0) self.wipe_ends.reverse() (reachable, ik_goal) = self.aul.full_ik_check(self.wipe_ends[1]) if not reachable: rospy.loginfo( "Cannot find approach for final wipe position, please try another" ) self.wt_log_out.publish( data= "Cannot find approach for final wipe position, please try another" ) return None else: rospy.loginfo("Received End position for wiping action") self.wt_log_out.publish( data="Received End position for wiping action") ####### REMOVED AND REPLACED WITH ALIGN FUNCTION ############## self.wipe_ends[0], self.wipe_ends[1] = self.align_poses( self.wipe_ends[0], self.wipe_ends[1]) self.aul.update_frame(self.wipe_appr_seed) appr = self.aul.find_approach(self.wipe_appr_seed, 0.15) appr.pose.orientation = self.wipe_ends[1].pose.orientation prepared = self.aul.move_arm_to(appr) if prepared: #self.aul.advance_to_contact() self.aul.blind_move(self.wipe_ends[1]) traj = self.aul.build_trajectory(self.wipe_ends[1], self.wipe_ends[0]) wipe_traj = self.aul.build_follow_trajectory(traj) self.aul.wait_for_stop() self.wipe_started = False return wipe_traj #self.wipe(wipe_traj) else: rospy.loginfo( "Failure reaching start point, please try again") self.wt_log_out.publish( data="Failure reaching start point, please try again") def align_poses(self, ps1, ps2): self.aul.update_frame(ps1) ps2.header.stamp = rospy.Time.now() self.tfl.waitForTransform(ps2.header.frame_id, 'lh_utility_frame', rospy.Time.now(), rospy.Duration(3.0)) ps2_in_ps1 = self.tfl.transformPose('lh_utility_frame', ps2) ang = math.atan2(-ps2_in_ps1.pose.position.z, -ps2_in_ps1.pose.position.y) + (math.pi / 2) q_st_rot = transformations.quaternion_about_axis(ang, (1, 0, 0)) q_st_new = transformations.quaternion_multiply([ ps1.pose.orientation.x, ps1.pose.orientation.y, ps1.pose.orientation.z, ps1.pose.orientation.w ], q_st_rot) ps1.pose.orientation = Quaternion(*q_st_new) self.aul.update_frame(ps2) ps1.header.stamp = rospy.Time.now() self.tfl.waitForTransform(ps1.header.frame_id, 'lh_utility_frame', rospy.Time.now(), rospy.Duration(3.0)) ps1_in_ps2 = self.tfl.transformPose('lh_utility_frame', ps1) ang = math.atan2(ps1_in_ps2.pose.position.z, ps1_in_ps2.pose.position.y) + (math.pi / 2) q_st_rot = transformations.quaternion_about_axis(ang, (1, 0, 0)) q_st_new = transformations.quaternion_multiply([ ps2.pose.orientation.x, ps2.pose.orientation.y, ps2.pose.orientation.z, ps2.pose.orientation.w ], q_st_rot) ps2.pose.orientation = Quaternion(*q_st_new) return ps1, ps2 def wipe_move(self, traj_goal, passes=4): times = [] for i in range(len(traj_goal.trajectory.points)): times.append(traj_goal.trajectory.points[i].time_from_start) count = 0 while count < passes: traj_goal.trajectory.points.reverse() for i in range(len(times)): traj_goal.trajectory.points[i].time_from_start = times[i] traj_goal.trajectory.header.stamp = rospy.Time.now() assert traj_goal.trajectory.points[0] != [] self.aul.l_arm_follow_traj_client.send_goal(traj_goal) self.aul.l_arm_follow_traj_client.wait_for_result( rospy.Duration(20)) rospy.sleep(0.5) # Pause at end of swipe count += 1 rospy.loginfo("Done Wiping") self.wt_log_out.publish(data="Done Wiping") hfm_pose = self.aul.hand_frame_move(-0.15) self.aul.blind_move(hfm_pose)
class RegistrationLoader(object): WORLD_FRAME = "odom_combined" HEAD_FRAME = "head_frame" def __init__(self): self.head_pose = None self.head_pc_reg = None self.head_frame_tf = None self.head_frame_bcast = TransformBroadcaster() self.tfl = TransformListener() self.init_reg_srv = rospy.Service("/initialize_registration", HeadRegistration, self.init_reg_cb) self.confirm_reg_srv = rospy.Service("/confirm_registration", ConfirmRegistration, self.confirm_reg_cb) self.head_registration_r = rospy.ServiceProxy("/head_registration_r", HeadRegistration) self.head_registration_l = rospy.ServiceProxy("/head_registration_l", HeadRegistration) self.feedback_pub = rospy.Publisher("/feedback", String) self.test_pose = rospy.Publisher("/test_head_pose", PoseStamped) self.reg_dir = rospy.get_param("~registration_dir", "") self.subject = rospy.get_param("~subject", None) def publish_feedback(self, msg): rospy.loginfo("[%s] %s" % (rospy.get_name(), msg)) self.feedback_pub.publish(msg) def init_reg_cb(self, req): # TODO REMOVE THIS FACE SIDE MESS self.publish_feedback("Performing Head Registration. Please Wait.") self.face_side = rospy.get_param("~face_side1", 'r') bag_str = self.reg_dir + '/' + '_'.join([self.subject, self.face_side, "head_transform"]) + ".bag" rospy.loginfo("[%s] Loading %s" %(rospy.get_name(), bag_str)) try: bag = rosbag.Bag(bag_str, 'r') for topic, msg, ts in bag.read_messages(): head_tf = msg assert (head_tf is not None), "Error reading head transform bagfile" bag.close() except Exception as e: self.publish_feedback("Registration failed: Error loading saved registration.") rospy.logerr("[%s] Cannot load registration parameters from %s:\r\n%s" % (rospy.get_name(), bag_str, e)) return (False, PoseStamped()) if self.face_side == 'r': head_registration = self.head_registration_r else: head_registration = self.head_registration_l try: rospy.loginfo("[%s] Requesting head registration for %s at pixel (%d, %d)." %(rospy.get_name(), self.subject, req.u, req.v)) self.head_pc_reg = head_registration(req.u, req.v).reg_pose if ((self.head_pc_reg.pose.position == Point(0.0, 0.0, 0.0)) and (self.head_pc_reg.pose.orientation == Quaternion(0.0, 0.0, 0.0, 1.0))): raise rospy.ServiceException("Unable to find a good match.") self.head_pc_reg = None except rospy.ServiceException as se: self.publish_feedback("Registration failed: %s" %se) return (False, PoseStamped()) pc_reg_mat = PoseConv.to_homo_mat(self.head_pc_reg) head_tf_mat = PoseConv.to_homo_mat(Transform(head_tf.transform.translation, head_tf.transform.rotation)) self.head_pose = PoseConv.to_pose_stamped_msg(pc_reg_mat**-1 * head_tf_mat) self.head_pose.header.frame_id = self.head_pc_reg.header.frame_id self.head_pose.header.stamp = self.head_pc_reg.header.stamp side = "right" if (self.face_side == 'r') else "left" self.publish_feedback("Registered head using %s cheek model, please check and confirm." %side) # rospy.loginfo('[%s] Head frame registered at:\r\n %s' %(rospy.get_name(), self.head_pose)) self.test_pose.publish(self.head_pose) return (True, self.head_pose) def confirm_reg_cb(self, req): if self.head_pose is None: raise rospy.ServiceException("Head has not been registered."); return False try: hp = copy.copy(self.head_pose) now = rospy.Time.now() + rospy.Duration(0.5) self.tfl.waitForTransform(self.WORLD_FRAME, hp.header.frame_id, now, rospy.Duration(10)) hp.header.stamp = now hp_world = self.tfl.transformPose(self.WORLD_FRAME, hp) pos = (hp_world.pose.position.x, hp_world.pose.position.y, hp_world.pose.position.z) quat = (hp_world.pose.orientation.x, hp_world.pose.orientation.y, hp_world.pose.orientation.z, hp_world.pose.orientation.w) self.head_frame_tf = (pos, quat) self.publish_feedback("Head registration confirmed."); return True except Exception as e: rospy.logerr("[%s] Error: %s" %(rospy.get_name(), e)) raise rospy.ServiceException("Error confirming head registration.")
class ParticleFilter: """ The class that represents a Particle Filter ROS Node Attributes list: initialized: a Boolean flag to communicate to other class methods that initialization is complete base_frame: the name of the robot base coordinate frame (should be "base_link" for most robots) map_frame: the name of the map coordinate frame (should be "map" in most cases) odom_frame: the name of the odometry coordinate frame (should be "odom" in most cases) scan_topic: the name of the scan topic to listen to (should be "scan" in most cases) n_particles: the number of particles in the filter d_thresh: the amount of linear movement before triggering a filter update a_thresh: the amount of angular movement before triggering a filter update laser_max_distance: the maximum distance to an obstacle we should use in a likelihood calculation pose_listener: a subscriber that listens for new approximate pose estimates (i.e. generated through the rviz GUI) particle_pub: a publisher for the particle cloud laser_subscriber: listens for new scan data on topic self.scan_topic tf_listener: listener for coordinate transforms tf_broadcaster: broadcaster for coordinate transforms particle_cloud: a list of particles representing a probability distribution over robot poses current_odom_xy_theta: the pose of the robot in the odometry frame when the last filter update was performed. The pose is expressed as a list [x,y,theta] (where theta is the yaw) map: the map we will be localizing ourselves in. The map should be of type nav_msgs/OccupancyGrid """ def __init__(self): self.initialized = False # make sure we don't perform updates before everything is setup rospy.init_node( 'pf') # tell roscore that we are creating a new node named "pf" self.base_frame = "base_link" # the frame of the robot base self.map_frame = "map" # the name of the map coordinate frame self.odom_frame = "odom" # the name of the odometry coordinate frame self.scan_topic = "scan" # the topic where we will get laser scans from self.n_particles = 150 # the number of particles to use self.d_thresh = 0.2 # the amount of linear movement before performing an update self.a_thresh = math.pi / 6 # the amount of angular movement before performing an update self.laser_max_distance = 2.0 # maximum penalty to assess in the likelihood field model self.radius = 2 # ac 109_1 #self.radius = 1 # ac_109_2 self.num_best_particles = 5 # ac 109_1 # self.num_best_particles = 8 # ac 109_2 # standard deviation of random noise distribution (Gaussian) for updating particle with odom # self.sigma_random_noise_update_odom = 0.01 # parameter for trying to not require a good initial estimate self.sigma_random_noise_update_odom = 0.008 # standard deviation of p(z^k_t | x_t, map) self.sigma_hit_update_scan = 0.01 #self.sigma_hit_update_scan = 0.05 # parameter for trying to not require a good initial estimate (did not work) self.z_hit = 1 self.z_rand = 0 # Setup pubs and subs # pose_listener responds to selection of a new approximate robot location (for instance using rviz) rospy.Subscriber("initialpose", PoseWithCovarianceStamped, self.update_initial_pose) # publish the current particle cloud. This enables viewing particles in rviz. self.particle_pub = rospy.Publisher("particlecloud", PoseArray, queue_size=10) # laser_subscriber listens for data from the lidar rospy.Subscriber(self.scan_topic, LaserScan, self.scan_received) # enable listening for and broadcasting coordinate transforms self.tf_listener = TransformListener() self.tf_broadcaster = TransformBroadcaster() self.particle_cloud = [] # change use_projected_stable_scan to True to use point clouds instead of laser scans self.use_projected_stable_scan = False self.last_projected_stable_scan = None if self.use_projected_stable_scan: # subscriber to the odom point cloud rospy.Subscriber("projected_stable_scan", PointCloud, self.projected_scan_received) self.current_odom_xy_theta = [] self.occupancy_field = OccupancyField() self.transform_helper = TFHelper() self.initialized = True def update_robot_pose(self, timestamp): """ Update the estimate of the robot's pose given the updated particles. There are two logical methods for this: (1): compute the mean pose (2): compute the most likely pose (i.e. the mode of the distribution) """ # first make sure that the particle weights are normalized self.normalize_particles() # just to get started we will fix the robot's pose to always be at the origin # self.robot_pose = Pose() x_sum = 0 y_sum = 0 theta_sum = 0 # Take the average of the best particles to be the robot's pose estimate particles_most_likely = sorted(self.particle_cloud, key=lambda x: x.w, reverse=True) for p in particles_most_likely[0:self.num_best_particles]: x_sum += p.x y_sum += p.y theta_sum += p.theta # should not do this, for some reason messed up the yaw # theta_sin_sum += math.sin(p.theta) # theta_cos_sum += math.cos(p.theta) x_avg = x_sum / self.num_best_particles y_avg = y_sum / self.num_best_particles theta_avg = theta_sum / self.num_best_particles # theta_avg = math.atan2(theta_sin_sum, theta_cos_sum) / self.num_best_particles (this is bad) mean_pose = Particle(x_avg, y_avg, theta_avg) self.robot_pose = mean_pose.as_pose() self.transform_helper.fix_map_to_odom_transform( self.robot_pose, timestamp) def projected_scan_received(self, msg): self.last_projected_stable_scan = msg def update_particles_with_odom(self, msg): """ Update the particles using the newly given odometry pose. The function computes the value delta which is a tuple (x,y,theta) that indicates the change in position and angle between the odometry when the particles were last updated and the current odometry. msg: this is not really needed to implement this, but is here just in case. """ new_odom_xy_theta = self.transform_helper.convert_pose_to_xy_and_theta( self.odom_pose.pose) # compute the change in x,y,theta since our last update if self.current_odom_xy_theta: old_odom_xy_theta = self.current_odom_xy_theta delta = (new_odom_xy_theta[0] - self.current_odom_xy_theta[0], new_odom_xy_theta[1] - self.current_odom_xy_theta[1], new_odom_xy_theta[2] - self.current_odom_xy_theta[2]) self.current_odom_xy_theta = new_odom_xy_theta else: self.current_odom_xy_theta = new_odom_xy_theta return # Update the odom of the particles accordingly d = math.sqrt(delta[0]**2 + delta[1]**2) for p in self.particle_cloud: p.x += d * math.cos(p.theta) + normal( 0, self.sigma_random_noise_update_odom) p.y += d * math.sin(p.theta) + normal( 0, self.sigma_random_noise_update_odom) p.theta += delta[2] + normal(0, self.sigma_random_noise_update_odom) def resample_particles(self): """ Resample the particles according to the new particle weights. The weights stored with each particle should define the probability that a particular particle is selected in the resampling step. You may want to make use of the given helper function draw_random_sample. """ # make sure the distribution is normalized self.normalize_particles() # Sort the particles first particles_most_likely = sorted(self.particle_cloud, key=lambda x: x.w, reverse=True) new_particle_cloud = [] # Low variance resampler from Probabilistic Robotics p87 count_inv = 1.0 / self.n_particles # the case when particles have equal weights r_num = random.uniform( 0, 1) * count_inv # draw a number in the interval [0,1/M] for m in range(self.n_particles): # Repeatedly add fixed amount to 1/M to random number r where 1/M represents the case where particles have # equal weights u = r_num + m * count_inv c = 0 # cumulative weights of particles for particle in particles_most_likely: c += particle.w # Add the first particle i such that the sum of weights of all particles from 0->i >= u if c >= u: new_particle_cloud.append(deepcopy(particle)) break self.particle_cloud = new_particle_cloud def update_particles_with_laser(self, msg): """ Updates the particle weights in response to the scan contained in the msg """ scans = msg.ranges for particle in self.particle_cloud: total_prob = 1 for angle, scan in enumerate(scans): if not math.isinf(scan): # convert scan measurement from view of the particle to map x_scan = particle.x + scan * math.cos(particle.theta + math.radians(angle)) y_scan = particle.y + scan * math.sin(particle.theta + math.radians(angle)) d = self.occupancy_field.get_closest_obstacle_distance( x_scan, y_scan) # Compute p(z^k_t | x_t, map) p_z_hit = self.z_hit * math.exp( -d**2 / (2 * (self.sigma_hit_update_scan**2)) ) / (self.sigma_hit_update_scan * math.sqrt(2 * math.pi)) p_z_rand = self.z_rand / self.laser_max_distance # z_random / z_max Probabilistic Robotics p143 p_z = p_z_hit + p_z_rand # We sum the cube of the probability # total_prob += p_z ** 3 # trying to make the model not require a good initial estimate total_prob += p_z**6 # total_prob = total_prob/len(msg.ranges) # It works better not to average -> converge faster # Reassign weight with newly computed p(z_t | x_t, map) particle.w = total_prob self.normalize_particles() @staticmethod def draw_random_sample(choices, probabilities, n): """ Return a random sample of n elements from the set choices with the specified probabilities choices: the values to sample from represented as a list probabilities: the probability of selecting each element in choices represented as a list n: the number of samples """ values = np.array(range(len(choices))) probs = np.array(probabilities) bins = np.add.accumulate(probs) inds = values[np.digitize(random_sample(n), bins)] samples = [] for i in inds: samples.append(deepcopy(choices[int(i)])) return samples def update_initial_pose(self, msg): """ Callback function to handle re-initializing the particle filter based on a pose estimate. These pose estimates could be generated by another ROS Node or could come from the rviz GUI """ xy_theta = self.transform_helper.convert_pose_to_xy_and_theta( msg.pose.pose) self.initialize_particle_cloud(msg.header.stamp, xy_theta) def initialize_particle_cloud(self, timestamp, xy_theta=None): """ Initialize the particle cloud. Arguments xy_theta: a triple consisting of the mean x, y, and theta (yaw) to initialize the particle cloud around. If this input is omitted, the odometry will be used """ if xy_theta is None: xy_theta = self.transform_helper.convert_pose_to_xy_and_theta( self.odom_pose.pose) self.particle_cloud = [] self.particle_cloud.append( Particle(xy_theta[0], xy_theta[1], xy_theta[2])) for p in range(self.n_particles - 1): p_yaw = random.uniform(0, 2 * math.pi) radius = random.uniform(0, 1) * self.radius theta_facing = random.uniform(0, 2 * math.pi) # Forward x axis of Neato is x p_x = radius * math.sin(theta_facing) + xy_theta[0] p_y = radius * math.cos(theta_facing) + xy_theta[1] self.particle_cloud.append(Particle(p_x, p_y, p_yaw)) self.normalize_particles() self.update_robot_pose(timestamp) def normalize_particles(self): """ Make sure the particle weights define a valid distribution (i.e. sum to 1.0) """ total_weight = 0 for p in self.particle_cloud: total_weight += p.w for p in self.particle_cloud: p.w = p.w / total_weight def publish_particles(self, msg): particles_conv = [] for p in self.particle_cloud: particles_conv.append(p.as_pose()) # actually send the message so that we can view it in rviz self.particle_pub.publish( PoseArray(header=Header(stamp=rospy.Time.now(), frame_id=self.map_frame), poses=particles_conv)) def scan_received(self, msg): """ This is the default logic for what to do when processing scan data. Feel free to modify this, however, we hope it will provide a good guide. The input msg is an object of type sensor_msgs/LaserScan """ if not (self.initialized): # wait for initialization to complete return if not (self.tf_listener.canTransform( self.base_frame, msg.header.frame_id, msg.header.stamp)): # need to know how to transform the laser to the base frame # this will be given by either Gazebo or neato_node return # wait a little while to see if the transform becomes available. This fixes a race # condition where the scan would arrive a little bit before the odom to base_link transform # was updated. self.tf_listener.waitForTransform(self.base_frame, self.odom_frame, msg.header.stamp, rospy.Duration(0.5)) if not (self.tf_listener.canTransform(self.base_frame, self.odom_frame, msg.header.stamp)): # need to know how to transform between base and odometric frames # this will eventually be published by either Gazebo or neato_node return # calculate pose of laser relative to the robot base p = PoseStamped( header=Header(stamp=rospy.Time(0), frame_id=msg.header.frame_id)) self.laser_pose = self.tf_listener.transformPose(self.base_frame, p) # find out where the robot thinks it is based on its odometry p = PoseStamped(header=Header(stamp=msg.header.stamp, frame_id=self.base_frame), pose=Pose()) self.odom_pose = self.tf_listener.transformPose(self.odom_frame, p) # store the the odometry pose in a more convenient format (x,y,theta) new_odom_xy_theta = self.transform_helper.convert_pose_to_xy_and_theta( self.odom_pose.pose) if not self.current_odom_xy_theta: self.current_odom_xy_theta = new_odom_xy_theta return if not (self.particle_cloud): # now that we have all of the necessary transforms we can update the particle cloud self.initialize_particle_cloud(msg.header.stamp) elif (math.fabs(new_odom_xy_theta[0] - self.current_odom_xy_theta[0]) > self.d_thresh or math.fabs(new_odom_xy_theta[1] - self.current_odom_xy_theta[1]) > self.d_thresh or math.fabs(new_odom_xy_theta[2] - self.current_odom_xy_theta[2]) > self.a_thresh): # we have moved far enough to do an update! self.update_particles_with_odom(msg) # update based on odometry if self.last_projected_stable_scan: last_projected_scan_timeshift = deepcopy( self.last_projected_stable_scan) last_projected_scan_timeshift.header.stamp = msg.header.stamp self.scan_in_base_link = self.tf_listener.transformPointCloud( "base_link", last_projected_scan_timeshift) self.update_particles_with_laser(msg) # update based on laser scan self.update_robot_pose(msg.header.stamp) # update robot's pose self.resample_particles( ) # resample particles to focus on areas of high density # publish particles (so things like rviz can see them) self.publish_particles(msg)
class MarkerProcessor(object): def __init__(self, use_dummy_transform=False): rospy.init_node('star_center_positioning_node') if use_dummy_transform: self.odom_frame_name = "odom_dummy" else: self.odom_frame_name = "odom" self.marker_locators = {} self.add_marker_locator(MarkerLocator(0,(0.0,0.0),0)) self.add_marker_locator(MarkerLocator(1,(1.4/1.1,2.0/1.1),0)) self.marker_sub = rospy.Subscriber("ar_pose_marker", ARMarkers, self.process_markers) self.odom_sub = rospy.Subscriber("odom", Odometry, self.process_odom, queue_size=10) self.star_pose_pub = rospy.Publisher("STAR_pose",PoseStamped,queue_size=10) self.continuous_pose = rospy.Publisher("STAR_pose_continuous",PoseStamped,queue_size=10) self.tf_listener = TransformListener() self.tf_broadcaster = TransformBroadcaster() def add_marker_locator(self, marker_locator): self.marker_locators[marker_locator.id] = marker_locator def process_odom(self, msg): p = PoseStamped(header=Header(stamp=rospy.Time(0), frame_id=self.odom_frame_name), pose=msg.pose.pose) try: STAR_pose = self.tf_listener.transformPose("STAR", p) STAR_pose.header.stamp = msg.header.stamp self.continuous_pose.publish(STAR_pose) except Exception as inst: print "error is", inst def process_markers(self, msg): for marker in msg.markers: # do some filtering basd on prior knowledge # we know the approximate z coordinate and that all angles but yaw should be close to zero euler_angles = euler_from_quaternion((marker.pose.pose.orientation.x, marker.pose.pose.orientation.y, marker.pose.pose.orientation.z, marker.pose.pose.orientation.w)) angle_diffs = TransformHelpers.angle_diff(euler_angles[0],pi), TransformHelpers.angle_diff(euler_angles[1],0) if (marker.id in self.marker_locators and 2.4 <= marker.pose.pose.position.z <= 2.6 and fabs(angle_diffs[0]) <= .2 and fabs(angle_diffs[1]) <= .2): locator = self.marker_locators[marker.id] xy_yaw = locator.get_camera_position(marker) orientation_tuple = quaternion_from_euler(0,0,xy_yaw[2]) pose = Pose(position=Point(x=xy_yaw[0],y=xy_yaw[1],z=0), orientation=Quaternion(x=orientation_tuple[0], y=orientation_tuple[1], z=orientation_tuple[2], w=orientation_tuple[3])) # TODO: use markers timestamp instead of now() (unfortunately, not populated currently by ar_pose) pose_stamped = PoseStamped(header=Header(stamp=rospy.Time.now(),frame_id="STAR"),pose=pose) try: offset, quaternion = self.tf_listener.lookupTransform("/base_link", "/base_laser_link", rospy.Time(0)) except Exception as inst: print "Error", inst return # TODO: use frame timestamp instead of now() pose_stamped_corrected = deepcopy(pose_stamped) pose_stamped_corrected.pose.position.x -= offset[0]*cos(xy_yaw[2]) pose_stamped_corrected.pose.position.y -= offset[0]*sin(xy_yaw[2]) self.star_pose_pub.publish(pose_stamped_corrected) self.fix_STAR_to_odom_transform(pose_stamped_corrected) def fix_STAR_to_odom_transform(self, msg): """ Super tricky code to properly update map to odom transform... do not modify this... Difficulty level infinity. """ (translation, rotation) = TransformHelpers.convert_pose_inverse_transform(msg.pose) p = PoseStamped(pose=TransformHelpers.convert_translation_rotation_to_pose(translation,rotation),header=Header(stamp=rospy.Time(),frame_id="base_link")) try: self.tf_listener.waitForTransform("odom","base_link",rospy.Time(),rospy.Duration(1.0)) except Exception as inst: print "whoops", inst return print "got transform" self.odom_to_STAR = self.tf_listener.transformPose("odom", p) (self.translation, self.rotation) = TransformHelpers.convert_pose_inverse_transform(self.odom_to_STAR.pose) def broadcast_last_transform(self): """ Make sure that we are always broadcasting the last map to odom transformation. This is necessary so things like move_base can work properly. """ if not(hasattr(self,'translation') and hasattr(self,'rotation')): return self.tf_broadcaster.sendTransform(self.translation, self.rotation, rospy.get_rostime(), self.odom_frame_name, "STAR") def run(self): r = rospy.Rate(10) while not rospy.is_shutdown(): self.broadcast_last_transform() r.sleep()
class Mover: """ A very simple Roamer implementation for Thorvald. It simply goes straight until any obstacle is within 3 m distance and then just simply turns left. A purely reactive approach. """ def __init__(self): """ On construction of the object, create a Subscriber to listen to lasr scans and a Publisher to control the robot """ self.publisher = rospy.Publisher('/thorvald_001/teleop_joy/cmd_vel', Twist, queue_size=1) rospy.Subscriber("/thorvald_001/front_scan", LaserScan, self.callback) self.pose_pub = rospy.Publisher('/nearest_obstacle', PoseStamped, queue_size=1) self.listener = TransformListener() def callback(self, data): """ Callback called any time a new laser scan becomes available """ # some logging on debug level, not usually displayed rospy.logdebug("I heard %s", data.header.seq) # This find the closest of all laser readings min_dist = min(data.ranges) # let's already create an object of type Twist to # publish it later. All initialised to 0! t = Twist() # If anything is closer than 4 metres anywhere in the # scan, we turn away if min_dist < 2: t.angular.z = 1.0 else: # if all obstacles are far away, let's keep # moving forward at 0.8 m/s t.linear.x = 0.8 # publish to the topic that makes the robot actually move self.publisher.publish(t) # above in min_dist we found the nearest value, # but to display the position of the nearest value # we need to find which range index corresponds to that # min_value. # find the index of the nearest point # (trick from https://stackoverflow.com/a/11825864) # it really is a very Python-ic trick here using a getter # function on the range. Can also be done with a # classic for loop index_min = min(range(len(data.ranges)), key=data.ranges.__getitem__) # convert the obtained index to angle, using the # reported angle_increment, and adding that to the # angle_min, i.e. the angle the index 0 corresponds to. # (is negative, in fact -PI/2). alpha = data.angle_min + (index_min * data.angle_increment) # No time for some trigonometry to turn the # polar coordinates into cartesian coordinates # inspired by https://stackoverflow.com/a/20926435 # use trigonometry to create the point in laser # z = 0, in the frame of the laser laser_point_2d = [cos(alpha) * min_dist, sin(alpha) * min_dist, 0.0] # create an empty PoseStamped to be published later. pose = PoseStamped() # keep the frame ID (the entire header here) as read from # the sensor. This is general ROS practice, as it # propagates the recording time from the sensor and # the corrdinate frame of the sensor through to # other results we may be publishing based on the anaylysis # of the data of that sensor pose.header = data.header # fill in the slots from the points calculated above. # bit tedious to do it this way, but hey... pose.pose.position.x = laser_point_2d[0] pose.pose.position.y = laser_point_2d[1] pose.pose.position.z = laser_point_2d[2] # using my trick from https://github.com/marc-hanheide/jupyter-notebooks/blob/master/quaternion.ipynb # to convert to quaternion, so that the Pose always # points in the direction of the laser beam. # Not required if only the positions is # of relevance # (then just set pose.pose.orientation.w = 1.0 and leave # others as 0). pose.pose.orientation.x = 0.0 pose.pose.orientation.y = 0.0 pose.pose.orientation.z = sin(alpha / 2) pose.pose.orientation.w = cos(alpha / 2) # publish the pose so it can be visualised in rviz self.pose_pub.publish(pose) rospy.loginfo("The closest point in laser frame coords is at\n%s" % pose.pose.position) # now lets actually transform this pose into a robot # "base_link" frame. transformed_pose = self.listener.transformPose( "thorvald_001/base_link", pose) rospy.loginfo("The closest point in robot coords is at\n%s" % transformed_pose.pose.position)
class ArmByFtWrist(object): def __init__(self): rospy.loginfo("Initializing...") # Node Hz rate self.rate = 10 self.rate_changed = False self.send_goals = False # Limits self.min_x = 0.0 self.max_x = 0.6 self.min_y = -0.5 self.max_y = -0.05 self.min_z = -0.2 self.max_z = 0.2 # Force stuff self.fx_scaling = 0.0 self.fy_scaling = 0.0 self.fz_scaling = 0.0 self.fx_deadband = 0.0 self.fy_deadband = 0.0 self.fz_deadband = 0.0 # Torque stuff self.tx_scaling = 0.0 self.ty_scaling = 0.0 self.tz_scaling = 0.0 self.tx_deadband = 0.0 self.ty_deadband = 0.0 self.tz_deadband = 0.0 self.dyn_rec_srv = Server(HandshakeConfig, self.dyn_rec_callback) # Signs adjusting fx, fy, fz, tx(roll), ty(pitch), tz(yaw) # for the left hand of reemc right now # And axis flipping self.frame_fixer = WrenchFixer(1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 'x', 'y', 'z', 'x', 'y', 'z') # Goal to send to WBC self.tf_l = TransformListener() rospy.sleep(3.0) # Let the subscriber to its job self.initial_pose = self.get_initial_pose() self.current_pose = self.initial_pose self.last_pose_to_follow = deepcopy(self.current_pose) self.pose_pub = rospy.Publisher('/whole_body_kinematic_controler/arm_right_tool_link_goal', PoseStamped, queue_size=1) # Safe debugging goal self.debug_pose_pub = rospy.Publisher('/force_torqued_pose', PoseStamped, queue_size=1) # Goal to follow self.follow_sub = rospy.Subscriber('/pose_to_follow', PoseStamped, self.follow_pose_cb, queue_size=1) # Sensor input self.last_wrench = None self.wrench_sub = rospy.Subscriber('/right_wrist_ft', WrenchStamped, self.wrench_cb, queue_size=1) rospy.loginfo("Done initializing.") def dyn_rec_callback(self, config, level): """ :param config: :param level: :return: """ rospy.loginfo("Received reconf call: " + str(config)) # Node Hz rate if self.rate != config['rate']: self.rate_changed = True self.rate = config['rate'] self.send_goals = config['send_goals'] # Limits self.min_x = config['min_x'] self.max_x = config['max_x'] self.min_y = config['min_y'] self.max_y = config['max_y'] self.min_z = config['min_z'] self.max_z = config['max_z'] # Force stuff self.fx_scaling = config['fx_scaling'] self.fy_scaling = config['fy_scaling'] self.fz_scaling = config['fz_scaling'] self.fx_deadband = config['fx_deadband'] self.fy_deadband = config['fy_deadband'] self.fz_deadband = config['fz_deadband'] # Torque stuff self.tx_scaling = config['tx_scaling'] self.ty_scaling = config['ty_scaling'] self.tz_scaling = config['tz_scaling'] self.tx_deadband = config['tx_deadband'] self.ty_deadband = config['ty_deadband'] self.tz_deadband = config['tz_deadband'] return config def follow_pose_cb(self, data): if data.header.frame_id != '/world': rospy.logwarn( "Poses to follow should be in frame /world, transforming into /world...") world_ps = self.transform_pose_to_world( data.pose, from_frame=data.header.frame_id) ps = PoseStamped() ps.header.stamp = data.header.stamp ps.header.frame_id = '/world' ps.pose = world_ps self.last_pose_to_follow = ps else: self.last_pose_to_follow = data def transform_pose_to_world(self, pose, from_frame="arm_right_tool_link"): ps = PoseStamped() ps.header.stamp = self.tf_l.getLatestCommonTime("world", from_frame) ps.header.frame_id = "/" + from_frame # TODO: check pose being PoseStamped or Pose ps.pose = pose transform_ok = False while not transform_ok and not rospy.is_shutdown(): try: world_ps = self.tf_l.transformPose("/world", ps) transform_ok = True return world_ps except ExtrapolationException as e: rospy.logwarn( "Exception on transforming pose... trying again \n(" + str(e) + ")") rospy.sleep(0.05) ps.header.stamp = self.tf_l.getLatestCommonTime( "world", from_frame) def wrench_cb(self, data): self.last_wrench = data def get_initial_pose(self): zero_pose = Pose() zero_pose.orientation.w = 1.0 current_pose = self.transform_pose_to_world( zero_pose, from_frame="wrist_right_ft_link") return current_pose def get_abs_total_force(self, wrench_msg): f = wrench_msg.wrench.force return abs(f.x) + abs(f.y) + abs(f.z) def get_abs_total_torque(self, wrench_msg): t = wrench_msg.wrench.torque return abs(t.x) + abs(t.y) + abs(t.z) def run(self): while not rospy.is_shutdown() and self.last_wrench is None: rospy.sleep(0.2) r = rospy.Rate(self.rate) while not rospy.is_shutdown(): # To change the node rate if self.rate_changed: r = rospy.Rate(self.rate) self.rate_changed = False self.move_towards_force_torque_with_tf() r.sleep() def move_towards_force_torque_with_tf(self): fx, fy, fz = self.get_force_movement() rospy.loginfo( "fx, fy, fz: " + str((round(fx, 3), round(fy, 3), round(fz, 3)))) send_new_goal = False ps = Pose() if abs(fx) > self.fx_deadband: ps.position.x = (fx * self.fx_scaling) * self.frame_fixer.fx send_new_goal = True if abs(fy) > self.fy_deadband: ps.position.y = (fy * self.fy_scaling) * self.frame_fixer.fy send_new_goal = True if abs(fz) > self.fz_deadband: ps.position.z = (fz * self.fz_scaling) * self.frame_fixer.fz send_new_goal = True tx, ty, tz = self.get_torque_movement() rospy.loginfo( "tx, ty, tz: " + str((round(tx, 3), round(ty, 3), round(tz, 3)))) ori_change = False roll = pitch = yaw = 0.0 if abs(tx) > self.tx_deadband: roll += (tx * self.tx_scaling) * self.frame_fixer.tx send_new_goal = True ori_change = True if abs(ty) > self.ty_deadband: pitch += (ty * self.ty_scaling) * self.frame_fixer.ty send_new_goal = True ori_change = True if abs(tz) > self.tz_deadband: yaw += (tz * self.tz_scaling) * self.frame_fixer.tz send_new_goal = True ori_change = True # if not send_new_goal: # rospy.loginfo("Not moving because of force torque.") # return q = quaternion_from_euler(roll, pitch, yaw) ps.orientation = Quaternion(*q) # rospy.loginfo("Local pose modification: " + str(ps)) # this pose is the current wrist link # plus the modification of the scalings force_torqued_pose = self.transform_pose_to_world( ps, from_frame="wrist_right_ft_link") #rospy.loginfo("Force-torque'd pose: " + str(force_torqued_pose)) # Now we get the difference with the wrist_right_ft_link (this should be optimized) # And we add that to the last goal pose wps = Pose() wps.orientation.w = 1.0 wrist_right_ft_link_pose = self.transform_pose_to_world(wps, from_frame="wrist_right_ft_link") x_diff = force_torqued_pose.pose.position.x - wrist_right_ft_link_pose.pose.position.x y_diff = force_torqued_pose.pose.position.y - wrist_right_ft_link_pose.pose.position.y z_diff = force_torqued_pose.pose.position.z - wrist_right_ft_link_pose.pose.position.z self.current_pose.pose.position.x = self.last_pose_to_follow.pose.position.x + x_diff self.current_pose.pose.position.y = self.last_pose_to_follow.pose.position.y + y_diff self.current_pose.pose.position.z = self.last_pose_to_follow.pose.position.z + z_diff ori_change = True if ori_change: # force torque RPY o = force_torqued_pose.pose.orientation r_ft, p_ft, y_ft = euler_from_quaternion([o.x, o.y, o.z, o.w]) rospy.loginfo("ForceTorqued ori: " + str((round(r_ft, 3), round(p_ft, 3), round(y_ft, 3)))) # wrist ft link RPY o2 = wrist_right_ft_link_pose.pose.orientation r_w, p_w, y_w = euler_from_quaternion([o2.x, o2.y, o2.z, o2.w]) rospy.loginfo("WristIdentity ori: " + str((round(r_w, 3), round(p_w, 3), round(y_w, 3)))) # last pose to follow RPY o3 = self.last_pose_to_follow.pose.orientation r_lp, p_lp, y_lp = euler_from_quaternion([o3.x, o3.y, o3.z, o3.w]) rospy.loginfo("Lastpose ori: " + str((round(r_lp, 3), round(p_lp, 3), round(y_lp, 3)))) roll_diff = r_ft - r_w pitch_diff = p_ft - p_w yaw_diff = y_ft - y_w rospy.loginfo("Diffs ori: " + str((round(roll_diff, 3), round(pitch_diff, 3), round(yaw_diff, 3)))) # Substract the constant orientation transform from tool_link zero_tool_link = self.transform_pose_to_world( ps, from_frame="arm_right_tool_link") o4 = zero_tool_link.pose.orientation r_tl, p_tl, y_tl = euler_from_quaternion([o4.x, o4.y, o4.z, o4.w]) rospy.loginfo("tool link ori: " + str((round(r_tl, 3), round(p_tl, 3), round(y_tl, 3)))) final_roll = r_tl - roll_diff final_pitch = p_tl - pitch_diff final_yaw = y_tl - yaw_diff q2 = quaternion_from_euler(final_roll, final_pitch, final_yaw) self.current_pose.pose.orientation = Quaternion(*q2) # else: # rospy.loginfo("No change in ori") # self.current_pose.pose.orientation = self.last_pose_to_follow.pose.orientation # debug self.current_pose.pose.position.x = self.sanitize(self.current_pose.pose.position.x, self.min_x, self.max_x) self.current_pose.pose.position.y = self.sanitize(self.current_pose.pose.position.y, self.min_y, self.max_y) self.current_pose.pose.position.z = self.sanitize(self.current_pose.pose.position.z, self.min_z, self.max_z) #rospy.loginfo("Workspace pose:\n" + str(self.current_pose.pose)) if self.send_goals and send_new_goal: self.pose_pub.publish(self.current_pose) self.debug_pose_pub.publish(self.current_pose) def get_force_movement(self): f_x = self.last_wrench.wrench.force.__getattribute__( self.frame_fixer.x_axis) f_y = self.last_wrench.wrench.force.__getattribute__( self.frame_fixer.y_axis) f_z = self.last_wrench.wrench.force.__getattribute__( self.frame_fixer.z_axis) return f_x, f_y, f_z def get_torque_movement(self): t_x = self.last_wrench.wrench.torque.__getattribute__( self.frame_fixer.roll_axis) t_y = self.last_wrench.wrench.torque.__getattribute__( self.frame_fixer.pitch_axis) t_z = self.last_wrench.wrench.torque.__getattribute__( self.frame_fixer.yaw_axis) return t_x, t_y, t_z def sanitize(self, data, min_v, max_v): if data > max_v: return max_v if data < min_v: return min_v return data
class TFHelper(object): """ TFHelper Provides functionality to convert poses between various forms, compare angles in a suitable way, and publish needed transforms to ROS """ def __init__(self): self.tf_listener = TransformListener() self.tf_broadcaster = TransformBroadcaster() def convert_translation_rotation_to_pose(self, translation, rotation): """ Convert from representation of a pose as translation and rotation (Quaternion) tuples to a geometry_msgs/Pose message """ return Pose(position=Point(x=translation[0], y=translation[1], z=translation[2]), orientation=Quaternion(x=rotation[0], y=rotation[1], z=rotation[2], w=rotation[3])) def convert_pose_inverse_transform(self, pose): """ This is a helper method to invert a transform (this is built into the tf C++ classes, but ommitted from Python) """ transform = t.concatenate_matrices( t.translation_matrix( [pose.position.x, pose.position.y, pose.position.z]), t.quaternion_matrix([ pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w ])) inverse_transform_matrix = t.inverse_matrix(transform) return (t.translation_from_matrix(inverse_transform_matrix), t.quaternion_from_matrix(inverse_transform_matrix)) def convert_pose_to_xy_and_theta(self, pose): """ Convert pose (geometry_msgs.Pose) to a (x,y,yaw) tuple """ orientation_tuple = (pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w) angles = t.euler_from_quaternion(orientation_tuple) return (pose.position.x, pose.position.y, angles[2]) def covert_xy_and_theta_to_pose(self, x, y, theta): """ A helper function to convert a x, y, and theta values to a pose """ orientation_tuple = t.quaternion_from_euler(0, 0, theta) return Pose(position=Point(x=x, y=y, z=0), orientation=Quaternion(x=orientation_tuple[0], y=orientation_tuple[1], z=orientation_tuple[2], w=orientation_tuple[3])) def angle_normalize(self, z): """ convenience function to map an angle to the range [-pi,pi] """ return math.atan2(math.sin(z), math.cos(z)) def angle_diff(self, a, b): """ Calculates the difference between angle a and angle b (both should be in radians) the difference is always based on the closest rotation from angle a to angle b. examples: angle_diff(.1,.2) -> -.1 angle_diff(.1, 2*math.pi - .1) -> .2 angle_diff(.1, .2+2*math.pi) -> -.1 """ a = self.angle_normalize(a) b = self.angle_normalize(b) d1 = a - b d2 = 2 * math.pi - math.fabs(d1) if d1 > 0: d2 *= -1.0 if math.fabs(d1) < math.fabs(d2): return d1 else: return d2 def loop_around(self, num): return abs(num % 360) def fix_map_to_odom_transform(self, robot_pose, timestamp): """ This method constantly updates the offset of the map and odometry coordinate systems based on the latest results from the localizer. robot_pose should be of type geometry_msgs/Pose and timestamp is of type rospy.Time and represents the time at which the robot's pose corresponds. """ (translation, rotation) = \ self.convert_pose_inverse_transform(robot_pose) p = PoseStamped(pose=self.convert_translation_rotation_to_pose( translation, rotation), header=Header(stamp=timestamp, frame_id='base_link')) self.tf_listener.waitForTransform('base_link', 'odom', timestamp, rospy.Duration(1.0)) self.odom_to_map = self.tf_listener.transformPose('odom', p) (self.translation, self.rotation) = \ self.convert_pose_inverse_transform(self.odom_to_map.pose) def send_last_map_to_odom_transform(self): if not (hasattr(self, 'translation') and hasattr(self, 'rotation')): return self.tf_broadcaster.sendTransform(self.translation, self.rotation, rospy.get_rostime(), 'odom', 'map')
class ScenarioServer(object): _id = 0 _loaded = False _robot_poses = [] _distances = [0,0] def __init__(self, name): rospy.loginfo("Starting scenario server") self.robot = rospy.get_param("~robot", False) conf_file = find_resource("topological_navigation", "scenario_server.yaml")[0] rospy.loginfo("Reading config file: %s ..." % conf_file) with open(conf_file,'r') as f: conf = yaml.load(f) self._robot_start_node = conf["robot_start_node"] self._robot_goal_node = conf["robot_goal_node"] self._obstacle_node_prefix = conf["obstacle_node_prefix"] self._obstacle_types = conf["obstacle_types"] self._timeout = conf["success_metrics"]["nav_timeout"] rospy.loginfo(" ... done") self._insert_maps() self.tf = TransformListener() rospy.Service("~load", LoadTopoNavTestScenario, self.load_scenario) self.reset_pose = self.reset_pose_robot if self.robot else self.reset_pose_sim rospy.Service("~reset", Empty, self.reset) rospy.Service("~start", RunTopoNavTestScenario, self.start) rospy.loginfo("All done") def _insert_maps(self): map_dir = rospy.get_param("~map_dir", "") rospy.loginfo("Inserting maps into datacentre...") if map_dir == "": rospy.logwarn("No 'map_dir' given. Will not insert maps into datacentre and assume they are present already.") return y = YamlMapLoader() y.insert_maps(y.read_maps(map_dir)) rospy.loginfo("... done") def robot_callback(self, msg): self._robot_poses.append(msg) def _connect_port(self, port): """ Establish the connection with the given MORSE port""" sock = None for res in socket.getaddrinfo(HOST, port, socket.AF_UNSPEC, socket.SOCK_STREAM): af, socktype, proto, canonname, sa = res try: sock = socket.socket(af, socktype, proto) except socket.error: sock = None continue try: sock.connect(sa) except socket.error: sock.close() sock = None continue break return sock def _translate(self, msg, target_tf): t = self.tf.getLatestCommonTime(target_tf, msg.header.frame_id) msg.header.stamp=t new_pose=self.tf.transformPose(target_tf,msg) return new_pose def _clear_costmaps(self): try: s = rospy.ServiceProxy("/move_base/clear_costmaps", Empty) s.wait_for_service() s() except rospy.ServiceException as e: rospy.logerr(e) def _init_nav(self, pose): pub = rospy.Publisher('/initialpose', PoseWithCovarianceStamped, queue_size=1) rospy.sleep(1.0) initialPose = PoseWithCovarianceStamped() initialPose.header = pose.header initialPose.pose.pose = pose.pose p_cov = np.array([0.0]*36) initialPose.pose.covariance = tuple(p_cov.ravel().tolist()) pub.publish(initialPose) def _get_set_object_pose_command(self, agent, id, pose): new_pose = self._translate( msg=pose, target_tf="/world" ) return 'id%d simulation set_object_pose ["%s", "[%f, %f, 0.1]", "[%f, %f, %f, %f]"]\n' \ % (id, agent, new_pose.pose.position.x, new_pose.pose.position.y, \ new_pose.pose.orientation.w, new_pose.pose.orientation.x, new_pose.pose.orientation.y, new_pose.pose.orientation.z) def robot_start_dist(self, msg): self._distances = self._get_pose_distance(msg, self._robot_start_pose.pose) def reset_pose_robot(self): rospy.loginfo("Enabling freerun ...") try: s = rospy.ServiceProxy("/enable_motors", EnableMotors) s.wait_for_service() s(False) except (rospy.ROSInterruptException, rospy.ServiceException) as e: rospy.logwarn(e) rospy.loginfo("... enabled") while self._robot_start_node != rospy.wait_for_message("/current_node", String).data and not rospy.is_shutdown(): rospy.loginfo("+++ Please push the robot to '%s' +++" % self._robot_start_node) rospy.sleep(1) rospy.loginfo("+++ Robot at '%s' +++" % self._robot_start_node) while not rospy.is_shutdown(): sub = rospy.Subscriber("/robot_pose", Pose, self.robot_start_dist) rospy.loginfo("+++ Please confirm correct positioning with A button on joypad: distance %.2fm %.2fdeg +++" %(self._distances[0], self._distances[1])) if self._robot_start_node != rospy.wait_for_message("/current_node", String).data: self.reset_pose() return try: if rospy.wait_for_message("/teleop_joystick/action_buttons", action_buttons, timeout=1.).A: break except rospy.ROSException: pass sub.unregister() sub = None rospy.loginfo("+++ Position confirmed +++") rospy.loginfo("Enabling motors, resetting motor stop and barrier stopped ...") try: s = rospy.ServiceProxy("/enable_motors", EnableMotors) s.wait_for_service() s(True) s = rospy.ServiceProxy("/reset_motorstop", ResetMotorStop) s.wait_for_service() s() s = rospy.ServiceProxy("/reset_barrier_stop", ResetBarrierStop) s.wait_for_service() s() except (rospy.ROSInterruptException, rospy.ServiceException) as e: rospy.logwarn(e) rospy.loginfo("... enabled and reset") def _send_socket(self, sock, agent, pose): sock.send( self._get_set_object_pose_command( agent, self._id, pose ) ) res = sock.recv(4096) self._id += 1 if "FAILURE" in res: raise Exception(res) def _get_quaternion_distance(self, q1, q2): q1 = (q1.x, q1.y, q1.z, q1.w) q2 = (q2.x, q2.y, q2.z, q2.w) return (trans.euler_from_quaternion(q1)[2] - trans.euler_from_quaternion(q2)[2]) * 180/np.pi def _get_euclidean_distance(self, p1, p2): return np.sqrt(np.power(p2.x - p1.x, 2) + np.power(p2.y - p1.y, 2)) def _get_pose_distance(self, p1, p2): e = self._get_euclidean_distance(p1.position, p2.position) q = self._get_quaternion_distance(p1.orientation, p2.orientation) return e, q def reset_pose_sim(self): sock = self._connect_port(PORT) if not sock: raise Exception("Could not create socket connection to morse") # Clean up whole scene # Create pose outside of map clear_pose = PoseStamped( header=Header(stamp=rospy.Time.now(), frame_id="/map"), pose=Pose(position=Point(x=20, y=20, z=0.01)) ) # Moving obstacles to clear_pose for obstacle in self._obstacle_types: for idx in range(10): self._send_socket(sock, obstacle+str(idx), clear_pose) # Setting robot and obstacles to correct position self._send_socket(sock, "robot", self._robot_start_pose) if len(self._obstacle_poses) > 0: for idx, d in enumerate(self._obstacle_poses): try: obstacle = max([x for x in self._obstacle_types if x in d["name"]], key=len) except ValueError: rospy.logwarn("No obstacle specified for obstacle node '%s', will use '%s'." % (d["name"], self._obstacle_types[0])) obstacle = self._obstacle_types[0] rospy.loginfo("Adding obstacle %s%i" % (obstacle,idx)) self._send_socket(sock, obstacle+str(idx), d["pose"]) else: rospy.logwarn("No nodes starting with '%s' found in map. Assuming test without obstacles." % self._obstacle_node_prefix) sock.close() while not rospy.is_shutdown(): rpose = rospy.wait_for_message("/robot_pose", Pose) rospy.loginfo("Setting initial amcl pose ...") self._init_nav(self._robot_start_pose) dist = self._get_pose_distance(rpose, self._robot_start_pose.pose) if dist[0] < 0.1 and dist[1] < 10: break rospy.sleep(0.2) # self._robot_start_pose.header.stamp = rospy.Time.now() rospy.loginfo(" ... pose set.") def reset(self, req): if not self._loaded: rospy.logfatal("No scenario loaded!") return EmptyResponse() self.client.cancel_all_goals() rospy.loginfo("Resetting robot position...") self.reset_pose() self._clear_costmaps() rospy.loginfo("... reset successful") return EmptyResponse() def graceful_fail(self): res = False closest_node = rospy.wait_for_message("/closest_node", String).data rospy.loginfo("Closest node: %s" % closest_node) if closest_node != self._robot_start_node: rospy.loginfo("Using policy execution from %s to %s" % (closest_node, self._robot_start_node)) s = rospy.ServiceProxy("/get_simple_policy/get_route_to", GetRouteTo) s.wait_for_service() policy = s(self._robot_start_node) self.client.send_goal(ExecutePolicyModeGoal(route=policy.route)) self.client.wait_for_result(timeout=rospy.Duration(self._timeout)) res = self.client.get_state() == actionlib_msgs.msg.GoalStatus.SUCCEEDED self.client.cancel_all_goals() else: rospy.loginfo("Using topo nav from %s to %s" % (closest_node, self._robot_start_node)) rospy.loginfo("Starting topo nav client...") client = actionlib.SimpleActionClient("/topological_navigation", GotoNodeAction) client.wait_for_server() rospy.loginfo(" ... done") client.send_goal(GotoNodeGoal(target=self._robot_start_node)) client.wait_for_result(timeout=rospy.Duration(self._timeout)) res = client.get_state() == actionlib_msgs.msg.GoalStatus.SUCCEEDED client.cancel_all_goals() return res def start(self, req): rospy.loginfo("Starting test ...") if not self._loaded: rospy.logfatal("No scenario loaded!") return RunTopoNavTestScenarioResponse(False, False) self._robot_poses = [] grace_res = False sub = rospy.Subscriber("/robot_pose", Pose, self.robot_callback) rospy.loginfo("Sending goal to policy execution ...") print self._policy.route self.client.send_goal(ExecutePolicyModeGoal(route=self._policy.route)) t = time.time() rospy.loginfo("... waiting for result ...") print self._timeout nav_timeout = not self.client.wait_for_result(timeout=rospy.Duration(self._timeout)) elapsed = time.time() - t res = self.client.get_state() == actionlib_msgs.msg.GoalStatus.SUCCEEDED rospy.loginfo("... policy execution finished") self.client.cancel_all_goals() if not res: rospy.loginfo("Attempting graceful death ...") grace_res = self.graceful_fail() rospy.loginfo("... dead") sub.unregister() sub = None distance = self.get_distance_travelled(self._robot_poses) rospy.loginfo("... test done") return RunTopoNavTestScenarioResponse( nav_success=res, nav_timeout=nav_timeout, graceful_fail=grace_res, human_success=False, min_distance_to_human=0, distance_travelled=distance, travel_time=elapsed, mean_speed=distance/elapsed ) def get_distance_travelled(self, poses): distance = 0.0 for idx in range(len(poses))[1:]: distance += self._get_euclidean_distance(poses[idx].position, poses[idx-1].position) return distance def load_scenario(self, req): # No try except to have exception break the test s = rospy.ServiceProxy("/topological_map_manager/switch_topological_map", GetTopologicalMap) s.wait_for_service() topo_map = s(GetTopologicalMapRequest(pointset=req.pointset)).map self.pointset = req.pointset self._robot_start_pose = None self._obstacle_poses = [] found_end_node = False for node in topo_map.nodes: if node.name == self._robot_start_node: self._robot_start_pose = PoseStamped( header=Header(stamp=rospy.Time.now(), frame_id="/map"), pose=node.pose ) elif node.name == self._robot_goal_node: found_end_node = True elif node.name.startswith(self._obstacle_node_prefix): self._obstacle_poses.append({ "name": node.name.lower(), "pose": PoseStamped( header=Header(stamp=rospy.Time.now(), frame_id="/map"), pose=node.pose ) }) if self._robot_start_pose == None: raise Exception("Topological map '%s' does not contain start node '%s'." % (req.pointset, self._robot_start_node)) if not found_end_node: raise Exception("Topological map '%s' does not contain goal node '%s'." % (req.pointset, self._robot_goal_node)) rospy.loginfo("Starting policy execution client...") # Has to be done here because the policy execution server waits for a topo map. self.client = actionlib.SimpleActionClient("/topological_navigation/execute_policy_mode", ExecutePolicyModeAction) self.client.wait_for_server() rospy.loginfo(" ... started") self._loaded = True self.reset(None) # No try except to have exception break the test rospy.loginfo("Getting route ...") s = rospy.ServiceProxy("/get_simple_policy/get_route_to", GetRouteTo) s.wait_for_service() self._policy = s(self._robot_goal_node) rospy.loginfo(" ... done") return LoadTopoNavTestScenarioResponse()
class Unique_persion_detection(object): """detect the unique persion from the fused objects""" def __init__(self,node_name = "Unique_person"): self.node_name = node_name rospy.init_node(self.node_name,anonymous="true") # self.pub = rospy.Publisher("trajectory",Path,queue_size = 2) self.info_sub = rospy.Subscriber("/fused_detectionarray", DetectionArray, self.info_odom_callback) # fused_pointcloud # self.odom_sub = message_filters.Subscriber("/husky1_robot_pose",Odometry) self.info_pub = rospy.Publisher("fused_person",PointCloud, queue_size = 2) self.unique_pub = rospy.Publisher("Unique_person",PointStamped, queue_size = 2) # self.image_pub = rospy.Publisher("Abnormal_image",Image,queue_size = 2) #self.odom_sub = message_filters.Subscriber("/optris/thermal_image",Image) self.paths_pub = [] self.listener = TransformListener() self.paths = [] self.paths_size = 0 #self.pose = PoseStamped() self.trans_pose = PoseStamped() #self.pose_index = [] self.prob = [] self.pre_velocity_prob = [] self.velocity = [] self.V_magnitude = [] self.V_direction = [] self.Human_list = [] self.cv_bridge = CvBridge() self.Unique_l = 0 self.fused_pointcloud = PointCloud() self.unique_pointstamped = PointStamped() rospy.loginfo("Class has been instantiated!") def info_odom_callback(self,info): #,odom print("comesin") self.velocity[:] = [] self.V_direction = [] self.V_magnitude = [] self.prob = [] self.fused_pointcloud = PointCloud() my_awesome_pointcloud = PointCloud() #filling pointcloud header # header = std_msgs.msg.Header() # header.stamp = rospy.Time.now() # header.frame_id = 'map' # my_awesome_pointcloud.header = header if (info.size > 0): # anzahl_punkte = 10 # i got 10 points, for example # self.stelldaten_tabelle.header = self.h # self.stelldaten_tabelle.points = anzahl_punkte # self.stelldaten_tabelle.channels = 3 # point = Point() # self.pointcloud.header.stamp = rospy.Time.now() # self.pointcloud.header.frame_id = "map" # self.pointcloud.points = info.size # self.pointcloud.channels = 3 for i in range(info.size): # print("info.size: ") # print(info.size) # print("i: ") # print(i) # print("info.data[i].ptStamped.point.x" ) # print(info.data[i].ptStamped.point.x ) # self.pointcloud.points[i].x = info.data[i].ptStamped.point.x # self.pointcloud.points[i].y = info.data[i].ptStamped.point.y # self.pointcloud.points[i].z = info.data[i].ptStamped.point.z # self.pointcloud.points.append(info.data[i].ptStamped.point) # Point32(1.0, 1.0, 0.0) my_awesome_pointcloud.points.append(Point32(info.data[i].ptStamped.point.x, info.data[i].ptStamped.point.y, info.data[i].ptStamped.point.z)) print("awesome") # print(my_awesome_pointcloud.points[i]) # self.pointcloud.points.append( Point32(info.data[i].ptStamped.point.x, info.data[i].ptStamped.point.y, info.data[i].ptStamped.point.z) ) self.pointcloud = my_awesome_pointcloud self.pointcloud.header.stamp = rospy.Time.now() self.pointcloud.header.frame_id = "map" # self.pointcloud.points = info.size # self.pointcloud.channels = 3 # print(self.pointcloud) #rospy.loginfo("%s",info.size) # Store the path info for each detection and publish the path while getting the new message update. for entry in info.data: if entry.num > self.paths_size: for i in range(entry.num-self.paths_size): self.paths.append(Path()) #self.dynamics.append(Dynamics()) self.paths_pub.append(rospy.Publisher("~trajectory"+str(self.paths_size + i + 1),Path,queue_size = 1)) #self.dynamics_pub.append(rospy.Publisher("~dynamics")+str(self.paths_size + i + 1),Dynamics,queue_size = 1) #self.pose_index.append(0) self.paths_size = entry.num pose = PoseStamped() point_stamped = entry.ptStamped pose.header = point_stamped.header pose.pose.position = point_stamped.point pose.pose.orientation.w = 1 rospy.loginfo("Trajectories") try: # Make true the frame_id of the pose (ptStamped) self.trans_pose = self.listener.transformPose("map",pose) except(tf.Exception): rospy.loginfo("The transformation is not available right now!") self.paths[entry.num - 1].poses.append(self.trans_pose) self.paths[entry.num - 1].header.frame_id = 'map' self.paths_pub[entry.num - 1].publish(self.paths[entry.num - 1]) rospy.loginfo("Poses stored!") if len(self.paths[entry.num - 1].poses) > 3: pre_x = self.paths[entry.num - 1].poses[-2].pose.position.x pre_y = self.paths[entry.num - 1].poses[-2].pose.position.y pre_z = self.paths[entry.num - 1].poses[-2].pose.position.z pre_stamp = self.paths[entry.num - 1].poses[-2].header.stamp cur_x = self.paths[entry.num - 1].poses[-1].pose.position.x cur_y = self.paths[entry.num - 1].poses[-1].pose.position.y cur_z = self.paths[entry.num - 1].poses[-1].pose.position.z cur_stamp = self.paths[entry.num - 1].poses[-1].header.stamp duration = (cur_stamp - pre_stamp).to_sec() vx = (cur_x - pre_x) / duration vy = (cur_y - pre_y) / duration self.velocity.append([cur_x,cur_y,cur_z,vx,vy,entry.num]) #self.pose_index[entry.num - 1] += 1 #rospy.loginfo("published paths!") # for index in range(self.paths_size): # #self.velocity[:] = [] # if len(self.paths[index].poses) > 3: # num = index + 1 # pre_x = self.paths[index].poses[-2].pose.position.x # pre_y = self.paths[index].poses[-2].pose.position.y # pre_z = self.paths[index].poses[-2].pose.position.z # pre_stamp = self.paths[index].poses[-2].header.stamp # cur_x = self.paths[index].poses[-1].pose.position.x # cur_y = self.paths[index].poses[-1].pose.position.y # cur_z = self.paths[index].poses[-1].pose.position.z # cur_stamp = self.paths[index].poses[-1].header.stamp # duration = (cur_stamp - pre_stamp).to_sec() # vx = (cur_x - pre_x) / duration # vy = (cur_y - pre_y) / duration # self.velocity.append([cur_x,cur_y,cur_z,vx,vy,num]) # else: # # pass # odom_x = odom.pose.pose.position.x # odom_y = odom.pose.pose.position.y # odom_z = odom.pose.pose.position.z odom_x = 0.0 odom_y = 0.0 odom_z = 0.0 #rospy.loginfo("There") '''odom_x = odom.height odom_y = odom.width odom_z = 1 ''' self.info = self.velocity.append([odom_x,odom_y,odom_z]) # append # rospy.loginfo("Velocity stored!") #rospy.loginfo("The current velocity info are %s",self.velocity) if (len(self.velocity) > 1): Human_num = len(self.velocity) - 1 for i in np.arange(Human_num): self.Human_list.append([self.velocity[i][5]]) self.V_magnitude.append(np.sqrt(np.square(self.velocity[i][3]) + np.square(self.velocity[i][4]))) vector1 = [self.velocity[i][3], self.velocity[i][4]] vector1 = vector1 / np.linalg.norm(vector1) # vector2 = [self.velocity[Human_num][0] - self.velocity[i][0], self.velocity[Human_num][1] - self.velocity[i][1]] # vector2 = vector2 / np.linalg.norm(vector2) # self.V_direction.append(np.dot(vector1, vector2)/(np.linalg.norm(vector1)*np.linalg.norm(vector2))) self.V_direction.append(np.dot(vector1,[0, -1])/(np.linalg.norm(vector1)*np.linalg.norm([0, -1]))) Unique_v = self.crf(self.V_magnitude) #print Unique_v #rospy.lginfo("HI") print self.V_direction Unique_d = self.crf(self.V_direction) print Unique_d Unique_d = self.normalize(np.exp(np.dot(-1,Unique_d))) print Unique_d #rospy.loginfo(len(self.V_direction)) #rospy.loginfo(len(self.V_magnitude)) if (np.var(Unique_d) == 0 or np.var(Unique_v)==0): [w_v,w_d] = [0.5,0.5] else: [w_v, w_d] = self.normalize([np.var(Unique_v), np.var(Unique_d)]) #print w_v #print w_d #Unique = self.softmax(np.array(Unique_v) * np.array(w_v) + np.array(Unique_d) * np.array(w_d)) Unique = np.array(Unique_v) * np.array(w_v) + np.array(Unique_d) * np.array(w_d) Unique = list(Unique) # print Unique #print(len(self.velocity)) #rospy.loginfo(Unique.index(max(Unique))) #print Unique #print(len(self.velocity)) #rospy.loginfo(Unique.index(max(Unique))) #self.Unique_l = self.velocity[Unique.index(max(Unique))][5] # rospy.loginfo("The Abnormal Object's label detected in husky1 camera is %s",self.Unique_l) #print("HELLO!")\ list1 = [] for (i,j)in zip(self.velocity[:-1],range(len(Unique))): # print("The num is %s",i[5]) self.prob.append(i[5]) self.prob.append(Unique[j]) print("prob are %s",self.prob) if not self.pre_velocity_prob: self.pre_velocity_prob = self.prob # print("HI Pre!") elif self.pre_velocity_prob: # print("Already here!") # print("prob 2 is %s",self.prob) # print(self.pre_velocity_prob) for i in range(len(self.prob)): if (i % 2 == 0): if self.prob[i] in self.pre_velocity_prob: self.prob[i + 1] = 0.6 * self.prob[i + 1] + 0.4 * self.pre_velocity_prob[self.pre_velocity_prob.index(self.prob[i]) + 1] print(self.prob[i+1]) else: self.prob[i + 1] = 0.6 * self.prob[i + 1] #print("change the probability!") for i in range(len(self.prob)): if (i % 2 == 0): list1.append(self.prob[i+1]) #print("okkkkk!") self.Unique_l = self.prob[self.prob.index(max(list1)) - 1] rospy.loginfo("The Unique Object's label detected in global frame is %s",self.Unique_l) self.pre_velocity_prob = self.prob # print("hello there!") # print("hello'") # cv_image = self.cv_bridge.imgmsg_to_cv2(info.image,"bgr8") # for entry in info.data: # if (entry.num == self.Unique_l): # cv2.rectangle(cv_image,(int(entry.x),int(entry.y)),(int(entry.x + entry.width),int(entry.y + entry.height)),(255,0,0),2) # cv2.putText(cv_image,"Person:" + str(entry.num), (int(entry.x),int(entry.y)),0,5e-3*100,(0,255,0),2) # else: # cv2.rectangle(cv_image,(int(entry.x),int(entry.y)),(int(entry.x + entry.width),int(entry.y + entry.height)),(255,0,0),2) # cv2.putText(cv_image,"Person:" + str(entry.num), (int(entry.x),int(entry.y)),0,5e-3*100,(0,255,0),2) # ros_image = self.cv_bridge.cv2_to_imgmsg(cv_image) # self.image_pub.publish(ros_image) # print("haha") for entry in info.data: if (entry.num == self.Unique_l): self.unique_pointstamped.point.x = entry.ptStamped.point.x self.unique_pointstamped.point.y = entry.ptStamped.point.y self.unique_pointstamped.point.z = entry.ptStamped.point.z self.unique_pointstamped.header.stamp = rospy.Time.now() self.unique_pointstamped.header.frame_id = "map" self.unique_pub.publish(self.unique_pointstamped) else: pass self.info_pub.publish(self.pointcloud) else: rospy.logdebug("There is no person detected in the current frame!") def crf(self,a): # conditional random field in feature level m=[] for i in np.arange(np.size(a)): m.append(reduce(lambda x,y:x*y,a)/a[i]) Z = reduce(lambda x,y:x+y,m) neibor = np.divide(m,Z) # neibor = list(self.normalize(np.divide(1,neibor))) #if np.var(neibor) <= 0.01 and np.var(neibor) != 0: # threshold for unique definition # neibor=[] #print neibor return neibor def softmax(self, a): if len(a) == 0: return a elif len(a) == 1: a = [1] return a e_a = np.exp(a - np.max(a)) return e_a / e_a.sum() def normalize(self,a): if len(a) == 0: return a elif len(a) == 1: a = [1] return a sum = reduce(lambda x,y:x+y,a) for i in np.arange(np.shape(a)[0]): a[i] = a[i] / sum return a def normalize2(self,a): for i in np.arange(len(a)): a[i] = (a[i]-np.min(a))/(np.max(a)-np.min(a)) return a
class NormalApproach(): standoff = 0.368 #0.2 + 0.168 (dist from wrist to fingertips) frame = 'base_footprint' px = py = pz = 0; qx = qy = qz = 0; qw = 1; def __init__(self): rospy.init_node('normal_approach_right') rospy.Subscriber('norm_approach_right', PoseStamped, self.update_frame) rospy.Subscriber('r_hand_pose', PoseStamped, self.update_curr_pose) rospy.Subscriber('wt_lin_move_right',Float32, self.linear_move) self.goal_out = rospy.Publisher('wt_right_arm_pose_commands', PoseStamped) self.move_arm_out = rospy.Publisher('wt_move_right_arm_goals', PoseStamped) self.tf = TransformListener() self.tfb = TransformBroadcaster() self.wt_log_out = rospy.Publisher('wt_log_out', String ) def update_curr_pose(self, msg): self.currpose = msg; def update_frame(self, pose): self.standoff = 0.368 self.frame = pose.header.frame_id self.px = pose.pose.position.x self.py = pose.pose.position.y self.pz = pose.pose.position.z self.qx = pose.pose.orientation.x self.qy = pose.pose.orientation.y self.qz = pose.pose.orientation.z self.qw = pose.pose.orientation.w self.tfb.sendTransform((self.px,self.py,self.pz),(self.qx,self.qy,self.qz,self.qw), rospy.Time.now(), "pixel_3d_frame", self.frame) self.find_approach(pose) def find_approach(self, msg): self.pose_in = msg self.tf.waitForTransform('pixel_3d_frame','base_footprint', rospy.Time(0), rospy.Duration(3.0)) self.tfb.sendTransform((self.pose_in.pose.position.x, self.pose_in.pose.position.y, self.pose_in.pose.position.z),(self.pose_in.pose.orientation.x, self.pose_in.pose.orientation.y, self.pose_in.pose.orientation.z, self.pose_in.pose.orientation.w), rospy.Time.now(), "pixel_3d_frame", self.pose_in.header.frame_id) self.tf.waitForTransform('pixel_3d_frame','r_wrist_roll_link', rospy.Time.now(), rospy.Duration(3.0)) goal = PoseStamped() goal.header.frame_id = 'pixel_3d_frame' goal.header.stamp = rospy.Time.now() goal.pose.position.z = self.standoff goal.pose.orientation.x = 0 goal.pose.orientation.y = 0.5*math.sqrt(2) goal.pose.orientation.z = 0 goal.pose.orientation.w = 0.5*math.sqrt(2) #print "Goal:\r\n %s" %goal self.tf.waitForTransform(goal.header.frame_id, 'torso_lift_link', rospy.Time.now(), rospy.Duration(3.0)) appr = self.tf.transformPose('torso_lift_link', goal) # print "Appr: \r\n %s" %appr self.wt_log_out.publish(data="Normal Approach with right hand: Trying to move WITH motion planning") self.move_arm_out.publish(appr) def linear_move(self, msg): print "Linear Move: Right Arm: %s m Step" %msg.data self.tf.waitForTransform(self.currpose.header.frame_id, 'r_wrist_roll_link', self.currpose.header.stamp, rospy.Duration(3.0)) newpose = self.tf.transformPose('r_wrist_roll_link', self.currpose) newpose.pose.position.x += msg.data step_goal = self.tf.transformPose(self.currpose.header.frame_id, newpose) self.goal_out.publish(step_goal)
class Controller(): ActionTakeOff = 0 ActionHover = 1 ActionLand = 2 ActionAnimation = 3 def __init__(self): self.lastNavdata = Navdata() self.lastImu = Imu() self.lastMag = Vector3Stamped() self.current_pose = PoseStamped() self.current_odom = Odometry() self.lastState = State.Unknown self.command = Twist() self.drone_msg = ARDroneData() self.cmd_freq = 1.0 / 200.0 self.drone_freq = 1.0 / 200.0 self.action = Controller.ActionTakeOff self.previousDebugTime = rospy.get_time() self.pose_error = [0, 0, 0, 0] self.pidX = PID(0.35, 0.15, 0.025, -1, 1, "x") self.pidY = PID(0.35, 0.15, 0.025, -1, 1, "y") self.pidZ = PID(1.25, 0.1, 0.25, -1.0, 1.0, "z") self.pidYaw = PID(0.75, 0.1, 0.2, -1.0, 1.0, "yaw") self.scale = 1.0 self.goal = [-1, 0, 0, height, 0] #set it to center to start self.goal_rate = [0, 0, 0, 0, 0] # Use the update the goal on time self.achieved_goal = Goal( ) # Use this to store recently achieved goal, reference time-dependent self.next_goal = Goal() # next goal self.goal_done = False self.waypoints = None rospy.on_shutdown(self.on_shutdown) rospy.Subscriber("ardrone/navdata", Navdata, self.on_navdata) rospy.Subscriber("ardrone/imu", Imu, self.on_imu) rospy.Subscriber("ardrone/mag", Vector3Stamped, self.on_mag) rospy.Subscriber("arcontroller/goal", Goal, self.on_goal) rospy.Subscriber("arcontroller/waypoints", Waypoints, self.on_waypoints) rospy.Subscriber("qualisys/ARDrone/pose", PoseStamped, self.get_current_pose) rospy.Subscriber("qualisys/ARDrone/odom", Odometry, self.get_current_odom) self.pubTakeoff = rospy.Publisher('ardrone/takeoff', Empty, queue_size=1) self.pubLand = rospy.Publisher('ardrone/land', Empty, queue_size=1) self.pubCmd = rospy.Publisher('cmd_vel', Twist, queue_size=1) self.pubDroneData = rospy.Publisher('droneData', ARDroneData, queue_size=1) self.pubGoal = rospy.Publisher('current_goal', Goal, queue_size=1) self.setFlightAnimation = rospy.ServiceProxy( 'ardrone/setflightanimation', FlightAnim) self.commandTimer = rospy.Timer(rospy.Duration(self.cmd_freq), self.sendCommand) #self.droneDataTimer = rospy.Timer(rospy.Duration(self.drone_freq),self.sendDroneData) #self.goalTimer = rospy.Timer(rospy.Duration(self.drone_freq),self.sendCurrentGoal) self.listener = TransformListener() def get_current_pose(self, data): self.current_pose = data def get_current_odom(self, data): self.current_odom = data def on_imu(self, data): self.lastImu = data def on_mag(self, data): self.lastMag = data def on_navdata(self, data): self.lastNavdata = data if data.state != self.lastState: rospy.loginfo("State Changed: " + str(data.state)) self.lastState = data.state def on_shutdown(self): rospy.loginfo("Shutdown: try to land...") self.command = Twist() for i in range(0, 1000): self.pubLand.publish() self.pubCmd.publish(self.command) rospy.sleep(1) def on_goal(self, data): rospy.loginfo('New goal.') self.goal = [data.t, data.x, data.y, data.z, data.yaw] self.goal_done = False def sendCommand(self, event=None): self.command.linear.x = self.scale * self.pidX.update( 0.0, self.pose_error[0]) self.command.linear.y = self.scale * self.pidY.update( 0.0, self.pose_error[1]) self.command.linear.z = self.pidZ.update(0.0, self.pose_error[2]) self.command.angular.z = self.pidYaw.update(0.0, self.pose_error[3]) self.drone_msg = ARDroneData() self.drone_msg.header.stamp = rospy.get_rostime() self.drone_msg.header.frame_id = 'drone_data' self.drone_msg.cmd = self.command self.drone_msg.goal.t = rospy.get_time() self.drone_msg.goal.x = self.goal[1] self.drone_msg.goal.y = self.goal[2] self.drone_msg.goal.z = self.goal[3] self.drone_msg.goal.yaw = self.goal[4] self.drone_msg.tm = self.lastNavdata.tm self.pubDroneData.publish(self.drone_msg) self.pubCmd.publish(self.command) def sendDroneData(self, event=None): self.drone_msg = ARDroneData() self.drone_msg.header.stamp = rospy.get_rostime() self.drone_msg.header.frame_id = 'drone_data' #self.drone_msg.navdata = self.lastNavdata #self.drone_msg.imu = self.lastImu #self.drone_msg.mag = self.lastMag #self.drone_msg.pose = self.current_pose #self.drone_msg.odom = self.current_odom self.drone_msg.cmd = self.command self.drone_msg.goal.t = rospy.get_time() self.drone_msg.goal.x = self.goal[1] self.drone_msg.goal.y = self.goal[2] self.drone_msg.goal.z = self.goal[3] self.drone_msg.goal.yaw = self.goal[4] self.drone_msg.tm = self.lastNavdata.tm self.pubDroneData.publish(self.drone_msg) def sendCurrentGoal(self, event=None): current_goal = Goal() current_goal.t = rospy.get_time() current_goal.x = self.goal[1] current_goal.y = self.goal[2] current_goal.z = self.goal[3] current_goal.yaw = self.goal[4] self.pubGoal.publish(current_goal) def on_waypoints(self, data): rospy.loginfo('New waypoints.') self.waypoints = [] for d in range(data.len): self.waypoints.append([ data.waypoints[d].t, data.waypoints[d].x, data.waypoints[d].y, data.waypoints[d].z, data.waypoints[d].yaw ]) rospy.loginfo(self.waypoints) def waypoint_follower(self, points): current_index = 0 previous_index = current_index - 1 rospy.loginfo(points) # Get the time vector time_wp = [goal[0] for goal in points] self.goal = points[current_index] #get the first point dt_two_wp = time_wp[1] - time_wp[0] self.achieved_goal.t = points[0][0] self.achieved_goal.x = points[0][1] self.achieved_goal.y = points[0][2] self.achieved_goal.z = points[0][3] self.achieved_goal.yaw = points[0][4] self.next_goal = self.achieved_goal self.goal_rate = [1, 0, 0, 0, 0] minX = .05 minY = .05 time0_wp = rospy.get_time() time_achieved_goal = time0_wp while True: #for i in range(0,points.len()): #goal = points[i] # transform target world coordinates into local coordinates targetWorld = PoseStamped() t = self.listener.getLatestCommonTime("/ARDrone", "/mocap") if self.listener.canTransform("/ARDrone", "/mocap", t): # Get starting time time_current_goal = rospy.get_time() dt_current_achieve = time_current_goal - time_achieved_goal # # Update the continuous goal using: goal = rate*t+goal # current_goalX = self.goal_rate[1]*dt_current_achieve+self.achieved_goal.x # current_goalY = self.goal_rate[2]*dt_current_achieve+self.achieved_goal.y # current_goalZ = self.goal_rate[3]*dt_current_achieve+self.achieved_goal.z # current_goalYaw = self.goal_rate[4]*dt_current_achieve+self.achieved_goal.yaw # self.goal = [self.next_goal.t,current_goalX,current_goalY,current_goalZ,current_goalYaw] self.goal = points[current_index] targetWorld.header.stamp = t targetWorld.header.frame_id = "mocap" targetWorld.pose.position.x = self.goal[1] targetWorld.pose.position.y = self.goal[2] targetWorld.pose.position.z = self.goal[3] quaternion = tf.transformations.quaternion_from_euler( 0, 0, self.goal[4]) targetWorld.pose.orientation.x = quaternion[0] targetWorld.pose.orientation.y = quaternion[1] targetWorld.pose.orientation.z = quaternion[2] targetWorld.pose.orientation.w = quaternion[3] targetDrone = self.listener.transformPose( "/ARDrone", targetWorld) quaternion = (targetDrone.pose.orientation.x, targetDrone.pose.orientation.y, targetDrone.pose.orientation.z, targetDrone.pose.orientation.w) euler = tf.transformations.euler_from_quaternion(quaternion) # Define the pose_error to publish the command in fixed rate self.pose_error = [ targetDrone.pose.position.x, targetDrone.pose.position.y, targetDrone.pose.position.z, euler[2] ] # Run PID controller and send navigation message error_xy = math.sqrt(targetDrone.pose.position.x**2 + targetDrone.pose.position.y**2) time_current_wp = rospy.get_time() if self.goal[ 0] < 0: #-1 implying that waypoints is not time-dependent # goal t, x, y, z, yaw #self.goal = points[current_index] if (error_xy < 0.2 and math.fabs(targetDrone.pose.position.z) < 0.2 and math.fabs(euler[2]) < math.radians(5)): if (current_index < len(points) - 1): current_index += 1 self.goal = points[current_index] else: return else: # Check how long from the first point diff_time_wp = time_current_wp - time0_wp if diff_time_wp <= time_wp[-1] - time_wp[ 0]: # this time should be less than the time defined wp # Check the index of current goal based on rospy time and time vector in waypoints current_index = next(x for x, val in enumerate(time_wp) if val >= diff_time_wp) if current_index > 0: # Meaning current goal is passed, update new goal previous_index = current_index - 1 self.achieved_goal.t = points[previous_index][0] self.achieved_goal.x = points[previous_index][1] self.achieved_goal.y = points[previous_index][2] self.achieved_goal.z = points[previous_index][3] self.achieved_goal.yaw = points[previous_index][4] self.next_goal.t = points[current_index][0] self.next_goal.x = points[current_index][1] self.next_goal.y = points[current_index][2] self.next_goal.z = points[current_index][3] self.next_goal.yaw = points[current_index][4] self.goal_rate = [ (points[current_index][i] - points[previous_index][i]) / dt_two_wp for i in range(5) ] time_achieved_goal = time_current_wp else: self.achieved_goal.t = points[0][0] self.achieved_goal.x = points[0][1] self.achieved_goal.y = points[0][2] self.achieved_goal.z = points[0][3] self.achieved_goal.yaw = points[0][4] self.next_goal = self.achieved_goal self.goal_rate = [1, 0, 0, 0, 0] else: # time already passed compared to the last goal, keep the last goal self.goal = points[-1] self.goal_rate = [1, 0, 0, 0, 0] return #time = rospy.get_time() diff_time_log = current_time_wp - self.previousDebugTime if diff_time_log > 0.5: #log_msg = "Current pos:" + [targetDrone.pose.position.x, targetDrone.pose.position.y,targetDrone.pose.position.z] rospy.loginfo('--------------------------------------') rospy.loginfo('Control:%.2f,%.2f,%.2f,%.2f | bat: %.2f', self.command.linear.x, self.command.linear.y, self.command.linear.z, self.command.angular.z, self.lastNavdata.batteryPercent) rospy.loginfo( 'Current position: [%.2f, %.2f, %.2f, %.2f, %.2f]', diff_time_wp, self.current_pose.pose.position.x, self.current_pose.pose.position.y, self.current_pose.pose.position.z, euler[2]) rospy.loginfo('Current goal:%.2f,%.2f,%.2f,%.2f,%.2f', self.goal[0], self.goal[1], self.goal[2], self.goal[3], self.goal[4]) rospy.loginfo('Error: %.2f,%.2f,%.2f', error_xy, math.fabs(targetDrone.pose.position.z), math.fabs(euler[2])) rospy.loginfo('--------------------------------------') self.previousDebugTime = current_time_wp def go_to_goal(self, goal): #rospy.loginfo('Going to goal') #rospy.loginfo(goal) self.goal = goal # transform target world coordinates into local coordinates targetWorld = PoseStamped() t = self.listener.getLatestCommonTime("/ARDrone", "/mocap") if self.listener.canTransform("/ARDrone", "/mocap", t): # Get starting time startCmdTime = rospy.get_time() targetWorld.header.stamp = t targetWorld.header.frame_id = "mocap" targetWorld.pose.position.x = goal[1] targetWorld.pose.position.y = goal[2] targetWorld.pose.position.z = goal[3] quaternion = tf.transformations.quaternion_from_euler( 0, 0, goal[4]) targetWorld.pose.orientation.x = quaternion[0] targetWorld.pose.orientation.y = quaternion[1] targetWorld.pose.orientation.z = quaternion[2] targetWorld.pose.orientation.w = quaternion[3] targetDrone = self.listener.transformPose("/ARDrone", targetWorld) quaternion = (targetDrone.pose.orientation.x, targetDrone.pose.orientation.y, targetDrone.pose.orientation.z, targetDrone.pose.orientation.w) euler = tf.transformations.euler_from_quaternion(quaternion) # Define pose error to publish the command in fixed rate self.pose_error = [ targetDrone.pose.position.x, targetDrone.pose.position.y, targetDrone.pose.position.z, euler[2] ] error_xy = math.sqrt(targetDrone.pose.position.x**2 + targetDrone.pose.position.y**2) time = rospy.get_time() if time - self.previousDebugTime > 1: #log_msg = "Current pos:" + [targetDrone.pose.position.x, targetDrone.pose.position.y,targetDrone.pose.position.z] rospy.loginfo('--------------------------------------') rospy.loginfo('Control:%.2f,%.2f,%.2f,%.2f,bat:%.2f', self.command.linear.x, self.command.linear.y, self.command.linear.z, self.command.angular.z, self.lastNavdata.batteryPercent) rospy.loginfo('Current goal:%.2f,%.2f,%.2f,%.2f', goal[1], goal[2], goal[3], goal[4]) rospy.loginfo('Current pose:%.2f,%.2f,%.2f', self.current_pose.pose.position.x, self.current_pose.pose.position.y, self.current_pose.pose.position.z) rospy.loginfo('Error: %.2f,%.2f,%.2f', error_xy, math.fabs(targetDrone.pose.position.z), math.fabs(euler[2])) self.previousDebugTime = time if self.goal_done: rospy.loginfo("Goal done.") rospy.loginfo('-------------------------------------') if (error_xy < 0.2 and math.fabs(targetDrone.pose.position.z) < 0.2 and math.fabs(euler[2]) < math.radians(5)): self.goal_done = True else: self.goal_done = False def run(self): while not rospy.is_shutdown(): if self.action == Controller.ActionTakeOff: if self.lastState == State.Landed: pass #rospy.loginfo('Taking off.') #self.pubTakeoff.publish() elif self.lastState == State.Hovering or self.lastState == State.Flying or self.lastState == State.Flying2: self.action = Controller.ActionHover elif self.action == Controller.ActionLand: msg = Twist() self.pubCmd.publish(msg) self.pubLand.publish() elif self.action == Controller.ActionHover: if self.waypoints == None: #if self.goal_done == False: self.go_to_goal(self.goal) else: self.waypoint_follower(self.waypoints) self.waypoints = None rospy.sleep(0.01)
gripper = moveit_commander.MoveGroupCommander("gripper") rospy.sleep(1) arm.set_goal_tolerance(0.003) print "current pose: ", arm.get_current_pose() print "current rpy: ", arm.get_current_rpy() print "joints: ", arm.get_current_joint_values() print "============ Generating plan 1" pose_target = geometry_msgs.msg.PoseStamped() pose_target.pose.orientation = geometry_msgs.msg.Quaternion(*tf_conversions.transformations.quaternion_from_euler(0, 1.57, 0)) pose_target.pose.position.x = float(sys.argv[1]) if len(sys.argv) >= 2 else 0.30 pose_target.pose.position.y = float(sys.argv[2]) if len(sys.argv) >= 3 else 0.0 pose_target.pose.position.z = float(sys.argv[3]) if len(sys.argv) >= 4 else 0.06 pose_target.header.frame_id = "/arips_base" tp_base = tf_listener.transformPose("/base_link", pose_target) arm.set_pose_target(tp_base.pose) plan1 = arm.plan() arm.go() rospy.sleep(1)
class Controller(): Manual = 0 Automatic = 1 TakeOff = 2 def __init__(self): self.pubNav = rospy.Publisher('cmd_vel', Twist, queue_size=1) self.listener = TransformListener() rospy.Subscriber("joy", Joy, self._joyChanged) rospy.Subscriber("cmd_vel_telop", Twist, self._cmdVelTelopChanged) self.cmd_vel_telop = Twist() self.pidX = PID(20, 12, 0.0, -30, 30, "x") self.pidY = PID(-20, -12, 0.0, -30, 30, "y") self.pidZ = PID(15000, 3000.0, 1500.0, 10000, 60000, "z") self.pidYaw = PID(-50.0, 0.0, 0.0, -200.0, 200.0, "yaw") self.state = Controller.Manual self.targetZ = 0.5 self.lastJoy = Joy() def _cmdVelTelopChanged(self, data): self.cmd_vel_telop = data if self.state == Controller.Manual: self.pubNav.publish(data) def pidReset(self): self.pidX.reset() self.pidZ.reset() self.pidZ.reset() self.pidYaw.reset() def _joyChanged(self, data): if len(data.buttons) == len(self.lastJoy.buttons): delta = np.array(data.buttons) - np.array(self.lastJoy.buttons) print("Buton ok") #Button 1 if delta[0] == 1 and self.state != Controller.Automatic: print("Automatic!") thrust = self.cmd_vel_telop.linear.z print(thrust) self.pidReset() self.pidZ.integral = thrust / self.pidZ.ki self.targetZ = 0.5 self.state = Controller.Automatic #Button 2 if delta[1] == 1 and self.state != Controller.Manual: print("Manual!") self.pubNav.publish(self.cmd_vel_telop) self.state = Controller.Manual #Button 3 if delta[2] == 1: self.state = Controller.TakeOff print("TakeOff!") #Button 5 if delta[4] == 1: self.targetZ += 0.1 print(self.targetZ) #Button 6 if delta[5] == 1: self.targetZ -= 0.1 print(self.targetZ) self.lastJoy = data def run(self): thrust = 0 print("jello") while not rospy.is_shutdown(): if self.state == Controller.TakeOff: t = self.listener.getLatestCommonTime("/mocap", "/Nano_Mark") if self.listener.canTransform("/mocap", "/Nano_Mark", t): position, quaternion = self.listener.lookupTransform( "/mocap", "/Nano_Mark", t) if position[2] > 0.1 or thrust > 50000: self.pidReset() self.pidZ.integral = thrust / self.pidZ.ki self.targetZ = 0.5 self.state = Controller.Automatic thrust = 0 else: thrust += 100 msg = self.cmd_vel_telop msg.linear.z = thrust self.pubNav.publish(msg) if self.state == Controller.Automatic: # transform target world coordinates into local coordinates t = self.listener.getLatestCommonTime("/Nano_Mark", "/mocap") print(t) if self.listener.canTransform("/Nano_Mark", "/mocap", t): targetWorld = PoseStamped() targetWorld.header.stamp = t targetWorld.header.frame_id = "mocap" targetWorld.pose.position.x = 0 targetWorld.pose.position.y = 0 targetWorld.pose.position.z = self.targetZ quaternion = tf.transformations.quaternion_from_euler( 0, 0, 0) targetWorld.pose.orientation.x = quaternion[0] targetWorld.pose.orientation.y = quaternion[1] targetWorld.pose.orientation.z = quaternion[2] targetWorld.pose.orientation.w = quaternion[3] targetDrone = self.listener.transformPose( "/Nano_Mark", targetWorld) quaternion = (targetDrone.pose.orientation.x, targetDrone.pose.orientation.y, targetDrone.pose.orientation.z, targetDrone.pose.orientation.w) euler = tf.transformations.euler_from_quaternion( quaternion) #msg = self.cmd_vel_teleop msg.linear.x = self.pidX.update( 0.0, targetDrone.pose.position.x) msg.linear.y = self.pidY.update( 0.0, targetDrone.pose.position.y) msg.linear.z = self.pidZ.update( 0.0, targetDrone.pose.position.z ) #self.pidZ.update(position[2], self.targetZ) msg.angular.z = self.pidYaw.update(0.0, euler[2]) #print(euler[2]) #print(msg.angular.z) self.pubNav.publish(msg) rospy.sleep(0.01)
class ParticleFilter: """ The class that represents a Particle Filter ROS Node Attributes list: initialized: a Boolean flag to communicate to other class methods that initializaiton is complete base_frame: the name of the robot base coordinate frame (should be "base_link" for most robots) map_frame: the name of the map coordinate frame (should be "map" in most cases) odom_frame: the name of the odometry coordinate frame (should be "odom" in most cases) scan_topic: the name of the scan topic to listen to (should be "scan" in most cases) n_particles: the number of particles in the filter d_thresh: the amount of linear movement before triggering a filter update a_thresh: the amount of angular movement before triggering a filter update laser_max_distance: the maximum distance to an obstacle we should use in a likelihood calculation pose_listener: a subscriber that listens for new approximate pose estimates (i.e. generated through the rviz GUI) particle_pub: a publisher for the particle cloud laser_subscriber: listens for new scan data on topic self.scan_topic tf_listener: listener for coordinate transforms tf_broadcaster: broadcaster for coordinate transforms particle_cloud: a list of particles representing a probability distribution over robot poses current_odom_xy_theta: the pose of the robot in the odometry frame when the last filter update was performed. The pose is expressed as a list [x,y,theta] (where theta is the yaw) map: the map we will be localizing ourselves in. The map should be of type nav_msgs/OccupancyGrid """ def __init__(self, test = False): self.test = test if self.test: print "Running particle filter in test mode" else: print "Running particle filter" self.initialized = False # make sure we don't perform updates before everything is setup rospy.init_node('pf') # tell roscore that we are creating a new node named "pf" self.base_frame = "base_link" # the frame of the robot base self.map_frame = "map" # the name of the map coordinate frame self.odom_frame = "odom" # the name of the odometry coordinate frame self.scan_topic = "scan" # the topic where we will get laser scans from self.n_particles = 20 # the number of particles to use self.d_thresh = 0.1 # the amount of linear movement before performing an update self.a_thresh = math.pi/6 # the amount of angular movement before performing an update self.laser_max_distance = 2.0 # maximum penalty to assess in the likelihood field model self.scan_count = 0 self.robot_pose = Pose(position=Point(x=.38, y=.6096,z=0), orientation=Quaternion(x=0, y=0, z=0, w=1)) # Setup pubs and subs # pose_listener responds to selection of a new approximate robot location (for instance using rviz) self.pose_listener = rospy.Subscriber("initialpose", PoseWithCovarianceStamped, self.update_initial_pose) # publish the current particle cloud. This enables viewing particles in rviz. self.particle_pub = rospy.Publisher("particlecloud", PoseArray) self.vel_pub = rospy.Publisher('/cmd_vel', Twist, queue_size=10) # laser_subscriber listens for data from the lidar self.laser_subscriber = rospy.Subscriber(self.scan_topic, LaserScan, self.scan_received) # enable listening for and broadcasting coordinate transforms self.tf_listener = TransformListener() self.tf_broadcaster = TransformBroadcaster() self.particle_cloud = [] self.ang_spread = 10.0/180.0 * math.pi self.lin_spread = .1 self.current_odom_xy_theta = [] #Set up constants for Guassian Probability self.variance = .1 self.gauss_constant = math.sqrt(2*math.pi) self.expected_value = 0 # request the map from the map server, the map should be of type nav_msgs/OccupancyGrid # We make a fake approximate map of an empty 2 by 2 square for testing to keep it simple. # If this worked better, we'd expand to a real OccupancyGrid, but we never got it to work better. map = OccupancyGrid() map.header.frame_id = '/odom' map.info.map_load_time = rospy.Time.now() map.info.resolution = .1 # The map resolution [m/cell] map.info.width = 288 map.info.height = 288 map.data = [[0 for _ in range(map.info.height)] for _ in range(map.info.width)] for row in range(map.info.height): map.data[0][row] = 1 map.data[map.info.width-1][row] = 1 for col in range(map.info.width): map.data[col][0] = 1 map.data[col][map.info.height -1] = 1 self.occupancy_field = OccupancyField(map) if self.test: print "Initialized" self.initialized = True def update_robot_pose(self): """ Update the estimate of the robot's pose given the updated particles. There are two logical methods for this: (1): compute the mean pose (level 2) (2): compute the most likely pose (i.e. the mode of the distribution) (level 1) """ if self.test: print "updating robot's pose" # first make sure that the particle weights are normalized self.normalize_particles() total_x = 0.0 total_y = 0.0 total_theta = 0.0 #calculates mean position of particle cloud according to particle weight #particles are normalized so sum of multiples will return mean for particle in self.particle_cloud: total_x += particle.x * particle.w total_y += particle.y * particle.w total_theta += particle.theta * particle.w total_theta = math.cos(total_theta/2) #set the robot pose to new position self.robot_pose = Pose(position=Point(x= +total_x, y=total_y,z=0), orientation=Quaternion(x=0, y=0, z=0, w=total_theta)) if self.test: print "updated pose:" print self.robot_pose def update_particles_with_odom(self, msg): """ Implement a simple version of this (Level 1) or a more complex one (Level 2) """ if self.test: print "Updating particles from odom" new_odom_xy_theta = TransformHelpers.convert_pose_to_xy_and_theta(self.odom_pose.pose) # compute the change in x,y,theta since our last update if self.current_odom_xy_theta: old_odom_xy_theta = self.current_odom_xy_theta delta = (new_odom_xy_theta[0] - self.current_odom_xy_theta[0], new_odom_xy_theta[1] - self.current_odom_xy_theta[1], new_odom_xy_theta[2] - self.current_odom_xy_theta[2]) self.current_odom_xy_theta = new_odom_xy_theta else: self.current_odom_xy_theta = new_odom_xy_theta return #function moves the particle cloud according to the odometry data #particle noise is added using gaussian distribution #standard deviation of gaussian dist was experimentally measured x_sd = .001 y_sd = .001 theta_sd = .1 for particle in self.particle_cloud: particle.x += np.random.normal(particle.x + delta[0], x_sd) particle.y += np.random.normal(particle.y + delta[1], y_sd) particle.theta += np.random.normal(particle.theta + delta[2], theta_sd) def map_calc_range(self,x,y,theta): """ Difficulty Level 3: implement a ray tracing likelihood model... Let me know if you are interested """ # TODO: nothing unless you want to try this alternate likelihood model pass def resample_particles(self): """ Resample the particles according to the new particle weights """ # make sure the distribution is normalized if self.test: print "Resampling Particles" #draw a random sample of particles from particle cloud #then normalize the remaining particles weights = [particle.w for particle in self.particle_cloud] self.particle_cloud = self.draw_random_sample( self.particle_cloud, weights, self.n_particles) self.normalize_particles() def update_particles_with_laser(self, msg): """ Updates the particle weights in response to the scan contained in the msg """ if self.test: print "Updating particles from laser scan" for particle in self.particle_cloud: #for each particle, get closest object distance and theta closest_particle_object_distance, closest_particle_object_theta = self.occupancy_field.get_closest_obstacle_distance(particle.x, particle.y) closest_actual_object_distance = 1000 for i in range(1, 360): if msg.ranges[i] > 0.0 and msg.ranges[i] < closest_actual_object_distance: closest_actual_object_distance = msg.ranges[i] closest_actual_object_theta = (i/360.0)*2*math.pi #update the particle's weight and theta particle.w = self.gauss_particle_probability(closest_particle_object_distance-closest_actual_object_distance) particle.theta = closest_actual_object_theta - closest_particle_object_theta def gauss_particle_probability(self, difference): """ Takes the difference between the actual closest object and the closest object to the particle guess and, based on the variance, returns the weight """ return (1/(self.variance*self.gauss_constant))*math.exp(-.5*((difference - self.expected_value)/self.variance)**2) @staticmethod def angle_normalize(z): """ convenience function to map an angle to the range [-pi,pi] """ return math.atan2(math.sin(z), math.cos(z)) @staticmethod def angle_diff(a, b): """ Calculates the difference between angle a and angle b (both should be in radians) the difference is always based on the closest rotation from angle a to angle b examples: angle_diff(.1,.2) -> -.1 angle_diff(.1, 2*math.pi - .1) -> .2 angle_diff(.1, .2+2*math.pi) -> -.1 """ a = ParticleFilter.angle_normalize(a) b = ParticleFilter.angle_normalize(b) d1 = a-b d2 = 2*math.pi - math.fabs(d1) if d1 > 0: d2 *= -1.0 if math.fabs(d1) < math.fabs(d2): return d1 else: return d2 @staticmethod def weighted_values(values, probabilities, size): """ Return a random sample of size elements form the set values with the specified probabilities values: the values to sample from (numpy.ndarray) probabilities: the probability of selecting each element in values (numpy.ndarray) size: the number of samples """ bins = np.add.accumulate(probabilities) return values[np.digitize(random_sample(size), bins)] @staticmethod def draw_random_sample(choices, probabilities, n): """ Return a random sample of n elements from the set choices with the specified probabilities choices: the values to sample from represented as a list probabilities: the probability of selecting each index represented as a list n: the number of samples """ values = np.array(range(len(choices))) probs = np.array(probabilities) bins = np.add.accumulate(probs) inds = values[np.digitize(random_sample(n), bins)] samples = [deepcopy(choices[ind]) for ind in inds] return samples def update_initial_pose(self, msg): """ Callback function to handle re-initializing the particle filter based on a pose estimate. These pose estimates could be generated by another ROS Node or could come from the rviz GUI """ if self.test: print "Updating Initial Pose" xy_theta = TransformHelpers.convert_pose_to_xy_and_theta(msg.pose.pose) self.initialize_particle_cloud(xy_theta) self.fix_map_to_odom_transform(msg) def initialize_particle_cloud(self, xy_theta=None): """ Initialize the particle cloud. Arguments xy_theta: a triple consisting of the mean x, y, and theta (yaw) to initialize the particle cloud around. If this input is ommitted, the odometry will be used """ if self.test: print "Initializing Cloud" if xy_theta == None: xy_theta = TransformHelpers.convert_pose_to_xy_and_theta(self.robot_pose) print self.robot_pose print xy_theta # raw_input() self.particle_cloud = [] # TODO create particles #create a normal distribution of particles around starting position #then normalize and update pose accordingly x_vals = np.random.normal(xy_theta[0], self.lin_spread, self.n_particles) y_vals = np.random.normal(xy_theta[1], self.lin_spread, self.n_particles) t_vals = np.random.normal(xy_theta[2], self.ang_spread, self.n_particles) self.particle_cloud = [Particle(x_vals[i], y_vals[i], t_vals[i], 1) for i in xrange(self.n_particles)] self.normalize_particles() self.update_robot_pose() # TODO(mary): create particles def normalize_particles(self): """ Make sure the particle weights define a valid distribution (i.e. sum to 1.0) """ total_weight = sum([particle.w for particle in self.particle_cloud]) * 1.0 for particle in self.particle_cloud: particle.w /= total_weight def publish_particles(self, msg): particles_conv = [] for p in self.particle_cloud: particles_conv.append(p.as_pose()) # actually send the message so that we can view it in rviz self.particle_pub.publish(PoseArray(header=Header(stamp=rospy.Time.now(),frame_id=self.map_frame),poses=particles_conv)) def scan_received(self, msg): self.scan_count +=1 """ This is the default logic for what to do when processing scan data. Feel free to modify this, however, I hope it will provide a good guide. The input msg is an object of type sensor_msgs/LaserScan """ if not(self.initialized): # wait for initialization to complete return if not(self.tf_listener.canTransform(self.base_frame,msg.header.frame_id,msg.header.stamp)): # need to know how to transform the laser to the base frame # this will be given by either Gazebo or neato_node return if not(self.tf_listener.canTransform(self.base_frame,self.odom_frame,msg.header.stamp)): # need to know how to transform between base and odometric frames # this will eventually be published by either Gazebo or neato_node return # calculate pose of laser relative ot the robot base p = PoseStamped(header=Header(stamp=rospy.Time(0),frame_id=msg.header.frame_id)) self.laser_pose = self.tf_listener.transformPose(self.base_frame,p) # find out where the robot thinks it is based on its odometry p = PoseStamped(header=Header(stamp=msg.header.stamp,frame_id=self.base_frame), pose=Pose()) self.odom_pose = self.tf_listener.transformPose(self.odom_frame, p) # store the the odometry pose in a more convenient format (x,y,theta) new_odom_xy_theta = TransformHelpers.convert_pose_to_xy_and_theta(self.odom_pose.pose) if not(self.particle_cloud): # now that we have all of the necessary transforms we can update the particle cloud self.initialize_particle_cloud() # cache the last odometric pose so we can only update our particle filter if we move more than self.d_thresh or self.a_thresh self.current_odom_xy_theta = new_odom_xy_theta # update our map to odom transform now that the particles are initialized self.fix_map_to_odom_transform(msg) # if self.test or self.scan_count % 1 is 0: print "updated pose:" print self.robot_pose elif (math.fabs(new_odom_xy_theta[0] - self.current_odom_xy_theta[0]) > self.d_thresh or math.fabs(new_odom_xy_theta[1] - self.current_odom_xy_theta[1]) > self.d_thresh or math.fabs(new_odom_xy_theta[2] - self.current_odom_xy_theta[2]) > self.a_thresh): # we have moved far enough to do an update! self.update_particles_with_odom(msg) # update based on odometry self.update_particles_with_laser(msg) # update based on laser scan self.update_robot_pose() # update robot's pose self.resample_particles() # resample particles to focus on areas of high density self.fix_map_to_odom_transform(msg) # update map to odom transform now that we have new particles # publish particles (so things like rviz can see them) self.publish_particles(msg) def fix_map_to_odom_transform(self, msg): """ Super tricky code to properly update map to odom transform... do not modify this... Difficulty level infinity. """ (translation, rotation) = TransformHelpers.convert_pose_inverse_transform(self.robot_pose) p = PoseStamped(pose=TransformHelpers.convert_translation_rotation_to_pose(translation,rotation),header=Header(stamp=msg.header.stamp,frame_id=self.base_frame)) self.odom_to_map = self.tf_listener.transformPose(self.odom_frame, p) (self.translation, self.rotation) = TransformHelpers.convert_pose_inverse_transform(self.odom_to_map.pose) def broadcast_last_transform(self): """ Make sure that we are always broadcasting the last map to odom transformation. This is necessary so things like move_base can work properly. """ if not(hasattr(self,'translation') and hasattr(self,'rotation')): return self.tf_broadcaster.sendTransform(self.translation, self.rotation, rospy.get_rostime(), self.odom_frame, self.map_frame)
def transformPose(self, target_frame, ps): now = rospy.Time.now() self.waitForTransform(target_frame, ps.header.frame_id, rospy.Time.now(), rospy.Duration(5.0)) ps.header.stamp = now return TransformListener.transformPose(self, target_frame, ps)
class ORKTabletop(object): """ Listens to the table and object messages from ORK. Provides ActionServer that assembles table and object into same message. NB - the table is an axis-aligned bounding box in the kinect's frame""" def __init__(self, name): self.sub = rospy.Subscriber("/recognized_object_array", RecognizedObjectArray, self.callback) self.pub = rospy.Publisher('/recognized_object_array_as_point_cloud', PointCloud2) self.pose_pub = rospy.Publisher('/recognized_object_array_as_pose_stamped', PoseStamped) # We listen for ORK's MarkerArray of tables on this topic self.table_topic = "/marker_tables" self.tl = TransformListener() # create messages that are used to publish feedback/result. # accessed by multiple threads self._result = TabletopResult() self.result_lock = threading.Lock() # used s.t. we don't return a _result message that hasn't been updated yet. self.has_data = False self._action_name = name self._as = actionlib.SimpleActionServer(self._action_name, TabletopAction, execute_cb=self.execute_cb, auto_start=False) self._as.start() # TODO: Is this really the best structure for handling the callbacks? # Would it be possible to have separate callbacks for table and objects, each updating a most-recently-seen variable? # Or maybe use the message_filters.TimeSynchronizer class if corresponding table/object data has identical timestamps? def callback(self, data): rospy.loginfo("Objects %d", data.objects.__len__()) table_corners = [] # obtain table_offset and table_pose to = rospy.wait_for_message(self.table_topic, MarkerArray); # obtain Table corners ... rospy.loginfo("Tables hull size %d", to.markers.__len__()) if not to.markers: rospy.loginfo("No tables detected") return else: # NB - D says that ORK has this set to filter based on height. # IIRC, it's 0.6-0.8m above whatever frame is set as the floor point_array_size = 4 # for the 4 corners of the bounding box for i in range (0, point_array_size): p = Point() p.x = to.markers[0].points[i].x p.y = to.markers[0].points[i].y p.z = to.markers[0].points[i].z table_corners.append(p) # this is a table pose at the edge close to the robot, in the center of x axis table_pose = PoseStamped() table_pose.header = to.markers[0].header table_pose.pose = to.markers[0].pose # Determine table dimensions rospy.loginfo('calculating table pose bounding box in frame: %s' % table_pose.header.frame_id) min_x = sys.float_info.max min_y = sys.float_info.max max_x = -sys.float_info.max max_y = -sys.float_info.max for i in range (table_corners.__len__()): if table_corners[i].x > max_x: max_x = table_corners[i].x if table_corners[i].y > max_y: max_y = table_corners[i].y if table_corners[i].x < min_x: min_x = table_corners[i].x if table_corners[i].y < min_y: min_y = table_corners[i].y table_dim = Point() # TODO: if we don't (can't!) compute the height, should we at least give it non-zero depth? # (would also require shifting the observed centroid down by table_dim.z/2) table_dim.z = 0.0 table_dim.x = abs(max_x - min_x) table_dim.y = abs(max_y - min_y) print "Dimensions: ", table_dim.x, table_dim.y # Temporary frame used for transformations table_link = 'table_link' # centroid of a table in table_link frame centroid = PoseStamped() centroid.header.frame_id = table_link centroid.header.stamp = table_pose.header.stamp centroid.pose.position.x = (max_x + min_x)/2. centroid.pose.position.y = (max_y + min_y)/2. centroid.pose.position.z = 0.0 centroid.pose.orientation.x = 0.0 centroid.pose.orientation.y = 0.0 centroid.pose.orientation.z = 0.0 centroid.pose.orientation.w = 1.0 # generate transform from table_pose to our newly-defined table_link tt = TransformStamped() tt.header = table_pose.header tt.child_frame_id = table_link tt.transform.translation = table_pose.pose.position tt.transform.rotation = table_pose.pose.orientation self.tl.setTransform(tt) self.tl.waitForTransform(table_link, table_pose.header.frame_id, table_pose.header.stamp, rospy.Duration(3.0)) if self.tl.canTransform(table_pose.header.frame_id, table_link, table_pose.header.stamp): centroid_table_pose = self.tl.transformPose(table_pose.header.frame_id, centroid) self.pose_pub.publish(centroid_table_pose) else: rospy.logwarn("No transform between %s and %s possible",table_pose.header.frame_id, table_link) return # transform each object into desired frame; add to list of clusters cluster_list = [] for i in range (data.objects.__len__()): rospy.loginfo("Point clouds %s", data.objects[i].point_clouds.__len__()) pc = PointCloud2() pc = data.objects[i].point_clouds[0] arr = pointclouds.pointcloud2_to_array(pc, 1) arr_xyz = pointclouds.get_xyz_points(arr) arr_xyz_trans = [] for j in range (arr_xyz.__len__()): ps = PointStamped(); ps.header.frame_id = table_link ps.header.stamp = table_pose.header.stamp ps.point.x = arr_xyz[j][0] ps.point.y = arr_xyz[j][1] ps.point.z = arr_xyz[j][2] if self.tl.canTransform(table_pose.header.frame_id, table_link, table_pose.header.stamp): ps_in_kinect_frame = self.tl.transformPoint(table_pose.header.frame_id, ps) else: rospy.logwarn("No transform between %s and %s possible",table_pose.header.frame_id, table_link) return arr_xyz_trans.append([ps_in_kinect_frame.point.x, ps_in_kinect_frame.point.y, ps_in_kinect_frame.point.z]) pc_trans = PointCloud2() pc_trans = pointclouds.xyz_array_to_pointcloud2(np.asarray([arr_xyz_trans]), table_pose.header.stamp, table_pose.header.frame_id) self.pub.publish(pc_trans) cluster_list.append(pc_trans) rospy.loginfo("cluster size %d", cluster_list.__len__()) # finally - save all data in the object that'll be sent in response to actionserver requests with self.result_lock: self._result.objects = cluster_list self._result.table_dims = table_dim self._result.table_pose = centroid_table_pose self.has_data = True def execute_cb(self, goal): rospy.loginfo('Executing ORKTabletop action') # want to get the NEXT data coming in, rather than the current one. with self.result_lock: self.has_data = False rr = rospy.Rate(1.0) while not rospy.is_shutdown() and not self._as.is_preempt_requested(): with self.result_lock: if self.has_data: break rr.sleep() if self._as.is_preempt_requested(): rospy.loginfo('%s: Preempted' % self._action_name) self._as.set_preempted() elif rospy.is_shutdown(): self._as.set_aborted() else: with self.result_lock: rospy.loginfo('%s: Succeeded' % self._action_name) self._as.set_succeeded(self._result)
rospy.sleep(1) arm.set_goal_tolerance(0.003) arm.set_max_velocity_scaling_factor(1.0) print("current pose: ", arm.get_current_pose()) print("current rpy: ", arm.get_current_rpy()) print("joints: ", arm.get_current_joint_values()) print("============ Generating plan 1") pose_target = geometry_msgs.msg.PoseStamped() pose_target.pose.orientation = geometry_msgs.msg.Quaternion(*tf_conversions.transformations.quaternion_from_euler(0, 1.57, 0)) pose_target.pose.position.x = float(sys.argv[1]) if len(sys.argv) >= 2 else 0.30 pose_target.pose.position.y = float(sys.argv[2]) if len(sys.argv) >= 3 else 0.0 pose_target.pose.position.z = float(sys.argv[3]) if len(sys.argv) >= 4 else 0.06 pose_target.header.frame_id = "/arips_base" tp_base = tf_listener.transformPose("/arm_base_link", pose_target) arm.set_pose_target(tp_base.pose) plan1 = arm.plan() arm.go() rospy.sleep(1)
class ParticleFilter: """ The class that represents a Particle Filter ROS Node Attributes list: initialized: a Boolean flag to communicate to other class methods that initializaiton is complete base_frame: the name of the robot base coordinate frame (should be "base_link" for most robots) map_frame: the name of the map coordinate frame (should be "map" in most cases) odom_frame: the name of the odometry coordinate frame (should be "odom" in most cases) scan_topic: the name of the scan topic to listen to (should be "scan" in most cases) n_particles: the number of particles in the filter d_thresh: the amount of linear movement before triggering a filter update a_thresh: the amount of angular movement before triggering a filter update laser_max_distance: the maximum distance to an obstacle we should use in a likelihood calculation pose_listener: a subscriber that listens for new approximate pose estimates (i.e. generated through the rviz GUI) particle_pub: a publisher for the particle cloud laser_subscriber: listens for new scan data on topic self.scan_topic tf_listener: listener for coordinate transforms tf_broadcaster: broadcaster for coordinate transforms particle_cloud: a list of particles representing a probability distribution over robot poses current_odom_xy_theta: the pose of the robot in the odometry frame when the last filter update was performed. The pose is expressed as a list [x,y,theta] (where theta is the yaw) map: the map we will be localizing ourselves in. The map should be of type nav_msgs/OccupancyGrid """ def __init__(self): self.initialized = False # make sure we don't perform updates before everything is setup rospy.init_node('pf') # tell roscore that we are creating a new node named "pf" self.base_frame = "base_link" # the frame of the robot base self.map_frame = "map" # the name of the map coordinate frame self.odom_frame = "odom" # the name of the odometry coordinate frame self.scan_topic = "scan" # the topic where we will get laser scans from self.n_particles = 300 # the number of particles to use self.model_noise_rate = 0.15 self.d_thresh = .2 # the amount of linear movement before performing an update self.a_thresh = math.pi / 6 # the amount of angular movement before performing an update self.laser_max_distance = 2.0 # maximum penalty to assess in the likelihood field model # TODO: define additional constants if needed # Setup pubs and subs # pose_listener responds to selection of a new approximate robot location (for instance using rviz) self.pose_listener = rospy.Subscriber("initialpose", PoseWithCovarianceStamped, self.update_initial_pose) # publish the current particle cloud. This enables viewing particles in rviz. self.particle_pub = rospy.Publisher("particlecloud", PoseArray, queue_size=10) # ????? # rospy.Subscriber('/simple_odom', Odometry, self.process_odom) # laser_subscriber listens for data from the lidar self.laser_subscriber = rospy.Subscriber(self.scan_topic, LaserScan, self.scan_received, queue_size=10) # enable listening for and broadcasting coordinate transforms self.tf_listener = TransformListener() self.tf_broadcaster = TransformBroadcaster() self.particle_cloud = [] self.current_odom_xy_theta = [] # [.0] * 3 # self.initial_particles = self.initial_list_builder() # self.particle_cloud = self.initialize_particle_cloud() print(self.particle_cloud) # self.current_odom_xy_theta = convert_pose_to_xy_and_theta(self.odom_pose.pose) # request the map from the map server, the map should be of type nav_msgs/OccupancyGrid # TODO: fill in the appropriate service call here. The resultant map should be assigned be passed # into the init method for OccupancyField # for now we have commented out the occupancy field initialization until you can successfully fetch the map mapa = obter_mapa() self.occupancy_field = OccupancyField(mapa) # self.update_particles_with_odom(msg) self.initialized = True def update_robot_pose(self): """ Update the estimate of the robot's pose given the updated particles. There are two logical methods for this: (1): compute the mean pose (2): compute the most likely pose (i.e. the mode of the distribution) """ # first make sure that the particle weights are normalized self.normalize_particles() # TODO: assign the lastest pose into self.robot_pose as a geometry_msgs.Pose object # just to get started we will fix the robot's pose to always be at the origin self.robot_pose = Pose() def update_particles_with_odom(self, msg): """ Update the particles using the newly given odometry pose. The function computes the value delta which is a tuple (x,y,theta) that indicates the change in position and angle between the odometry when the particles were last updated and the current odometry. msg: this is not really needed to implement this, but is here just in case. """ new_odom_xy_theta = convert_pose_to_xy_and_theta(self.odom_pose.pose) print(new_odom_xy_theta) # compute the change in x,y,theta since our last update if self.current_odom_xy_theta: new_odom_xy_theta = convert_pose_to_xy_and_theta(self.odom_pose.pose) old_odom_xy_theta = self.current_odom_xy_theta delta = (new_odom_xy_theta[0] - self.current_odom_xy_theta[0], new_odom_xy_theta[1] - self.current_odom_xy_theta[1], new_odom_xy_theta[2] - self.current_odom_xy_theta[2]) for p in self.particle_cloud: p.x += delta[0] p.y += delta[1] p.theta += delta[2] self.current_odom_xy_theta = new_odom_xy_theta else: self.current_odom_xy_theta = new_odom_xy_theta return # ???? # TODO: modify particles using delta for p in self.particle_cloud: r = math.atan2(delta[1], delta[0]) - old_odom_xy_theta[2] d = math.sqrt((delta[0] ** 2) + (delta[1] ** 2)) p.theta += r % 360 p.x += d * math.cos(p.theta) + normal(0, .1) p.y += d * math.sin(p.theta) + normal(0, .1) p.theta += (delta[2] - r + normal(0, .1)) % 360 # For added difficulty: Implement sample_motion_odometry (Prob Rob p 136) def map_calc_range(self,x,y,theta): """ Difficulty Level 3: implement a ray tracing likelihood model... Let me know if you are interested """ # TODO: nothing unless you want to try this alternate likelihood model pass def resample_particles(self): """ Resample the particles according to the new particle weights. The weights stored with each particle should define the probability that a particular particle is selected in the resampling step. You may want to make use of the given helper function draw_random_sample. """ # make sure the distribution is normalized # TODO: fill out the rest of the implementation self.particle_cloud = ParticleFilter.weighted_values(self.particle_cloud, [p.w for p in self.particle_cloud], len(self.particle_cloud)) for p in particle_cloud: p.w = 1 / len(self.particle_cloud) self.normalize_particles() def update_particles_with_laser(self, msg): """ Updates the particle weights in response to the scan contained in the msg """ # TODO: implement this for r in msg.ranges: for p in self.particle_cloud: p.w = 1 self.occupancy_field.get_particle_likelyhood(p, r, self.model_noise_rate) self.normalize_particles() self.resample_particles() @staticmethod def weighted_values(values, probabilities, size): """ Return a random sample of size elements from the set values with the specified probabilities values: the values to sample from (numpy.ndarray) probabilities: the probability of selecting each element in values (numpy.ndarray) size: the number of samples """ bins = np.add.accumulate(probabilities) print(size, bins) return values[np.digitize(random_sample(size), bins)] @staticmethod def draw_random_sample(choices, probabilities, n): """ Return a random sample of n elements from the set choices with the specified probabilities choices: the values to sample from represented as a list probabilities: the probability of selecting each element in choices represented as a list n: the number of samples """ values = np.array(range(len(choices))) probs = np.array(probabilities) bins = np.add.accumulate(probs) inds = values[np.digitize(random_sample(n), bins)] samples = [] for i in inds: samples.append(deepcopy(choices[int(i)])) return samples def update_initial_pose(self, msg): """ Callback function to handle re-initializing the particle filter based on a pose estimate. These pose estimates could be generated by another ROS Node or could come from the rviz GUI """ xy_theta = convert_pose_to_xy_and_theta(msg.pose.pose) self.initialize_particle_cloud(xy_theta) self.fix_map_to_odom_transform(msg) def initialize_particle_cloud(self, xy_theta=None): """ Initialize the particle cloud. Arguments xy_theta: a triple consisting of the mean x, y, and theta (yaw) to initialize the particle cloud around. If this input is ommitted, the odometry will be used """ if xy_theta == None: xy_theta = convert_pose_to_xy_and_theta(self.odom_pose.pose) # TODO create particles self.particle_cloud = self.initial_list_builder(xy_theta) self.normalize_particles() self.update_robot_pose() def normalize_particles(self): """ Make sure the particle weights define a valid distribution (i.e. sum to 1.0) """ # TODO: implement this w_sum = 0 for p in self.particle_cloud: w_sum += p.w for p in self.particle_cloud: p.w /= w_sum def publish_particles(self, msg): particles_conv = [] for p in self.particle_cloud: particles_conv.append(p.as_pose()) # actually send the message so that we can view it in rviz self.particle_pub.publish(PoseArray(header=Header(stamp=rospy.Time.now(), frame_id=self.map_frame), poses=particles_conv)) def scan_received(self, msg): """ This is the default logic for what to do when processing scan data. Feel free to modify this, however, I hope it will provide a good guide. The input msg is an object of type sensor_msgs/LaserScan """ if not(self.initialized): # wait for initialization to complete # print 1 return if not(self.tf_listener.canTransform(self.base_frame,msg.header.frame_id,rospy.Time(0))): # need to know how to transform the laser to the base frame # this will be given by either Gazebo or neato_node print 2 return if not(self.tf_listener.canTransform(self.base_frame,self.odom_frame,rospy.Time(0))): # need to know how to transform between base and odometric frames # this will eventually be published by either Gazebo or neato_node print 3 return # calculate pose of laser relative ot the robot base p = PoseStamped(header=Header(stamp=rospy.Time(0), frame_id=msg.header.frame_id)) self.laser_pose = self.tf_listener.transformPose(self.base_frame,p) # find out where the robot thinks it is based on its odometry p = PoseStamped(header=Header(stamp=rospy.Time(0), frame_id=self.base_frame), pose=Pose()) self.odom_pose = self.tf_listener.transformPose(self.odom_frame, p) # store the the odometry pose in a more convenient format (x,y,theta) new_odom_xy_theta = convert_pose_to_xy_and_theta(self.odom_pose.pose) print(self.current_odom_xy_theta) # Essa list não está sendo "refeita" / preenchida print(new_odom_xy_theta) if not(self.particle_cloud): # now that we have all of the necessary transforms we can update the particle cloud self.initialize_particle_cloud() # cache the last odometric pose so we can only update our particle filter if we move more than self.d_thresh or self.a_thresh self.current_odom_xy_theta = new_odom_xy_theta # update our map to odom transform now that the particles are initialized self.fix_map_to_odom_transform(msg) print(math.fabs(new_odom_xy_theta[0] - self.current_odom_xy_theta[0]), "hi") elif (math.fabs(new_odom_xy_theta[0] - self.current_odom_xy_theta[0]) > self.d_thresh or math.fabs(new_odom_xy_theta[1] - self.current_odom_xy_theta[1]) > self.d_thresh or math.fabs(new_odom_xy_theta[2] - self.current_odom_xy_theta[2]) > self.a_thresh): # we have moved far enough to do an update! ''' É AQUI!!!! ''' print('jorge') self.update_particles_with_odom(msg) # update based on odometry - FAZER self.update_particles_with_laser(msg) # update based on laser scan - FAZER self.update_robot_pose() # update robot's pose """ abaixo """ self.resample_particles() # resample particles to focus on areas of high density - FAZER self.fix_map_to_odom_transform(msg) # update map to odom transform now that we have new particles # publish particles (so things like rviz can see them) self.publish_particles(msg) def fix_map_to_odom_transform(self, msg): """ This method constantly updates the offset of the map and odometry coordinate systems based on the latest results from the localizer """ (translation, rotation) = convert_pose_inverse_transform(self.robot_pose) p = PoseStamped(pose=convert_translation_rotation_to_pose(translation,rotation), header=Header(stamp=rospy.Time(0),frame_id=self.base_frame)) self.odom_to_map = self.tf_listener.transformPose(self.odom_frame, p) (self.translation, self.rotation) = convert_pose_inverse_transform(self.odom_to_map.pose) def broadcast_last_transform(self): """ Make sure that we are always broadcasting the last map to odom transformation. This is necessary so things like move_base can work properly. """ if not(hasattr(self,'translation') and hasattr(self,'rotation')): return self.tf_broadcaster.sendTransform(self.translation, self.rotation, rospy.Time.now(), self.odom_frame, self.map_frame) def initial_list_builder(self, xy_theta): ''' Creates the initial particles list, using the super advanced methods provided to us by the one and only John Cena ''' initial_particles = [] for i in range(self.n_particles): p = Particle() p.x = gauss(xy_theta[0], 1) p.y = gauss(xy_theta[1], 1) p.theta = gauss(xy_theta[2], (math.pi / 2)) p.w = 1.0 / self.n_particles initial_particles.append(p) return initial_particles
class ProportionalControl: """ This class encapsulates and provides interface to a Proportional controller used to control the base """ def __init__(self, bot_base, ctrl_pub, configs): """ The constructor for ProportionalControl class. :param configs: configurations read from config file :param base_state: an object consisting of an instance of BaseState. :param ctrl_pub: a ros publisher used to publish velocity commands to base of the robot. :type configs: dict :type base_state: BaseState :type ctrl_pub: rospy.Publisher """ self.configs = configs self.bot_base = bot_base self.MAP_FRAME = self.configs.BASE.MAP_FRAME self.BASE_FRAME = self.configs.BASE.VSLAM.VSLAM_BASE_FRAME self.ctrl_pub = ctrl_pub self.rot_move_thr = ROT_MOVE_THR # threshold for linear.. if its more than this then we consider its # moving self.lin_move_thr = LIN_MOVE_THR self.rot_max_vel = self.configs.BASE.MAX_ABS_TURN_SPEED_P_CONTROLLER self.lin_max_vel = self.configs.BASE.MAX_ABS_FWD_SPEED_P_CONTROLLER self.translation_treshold = self.configs.BASE.TRANSLATION_TRESHOLD # threshold between which if error lies, we think of task being don self.rot_error_thr = ROT_ERR_THR self.dist_error_thr = LIN_ERR_THR self.vel_delta = VEL_DELTA self.hz = HZ self._transform_listener = TransformListener() self.ignore_collisions = False def _cmd_vel(self, lin_vel=0.0, rot_vel=0.0): # writting vel to robot cmd = Twist() cmd.angular.z = rot_vel cmd.linear.x = lin_vel self.ctrl_pub.publish(cmd) def stop(self): rospy.loginfo("Stopping base!") self._cmd_vel(lin_vel=0.0, rot_vel=0.0) self.bot_base.should_stop = False def _norm_pose(self, data): # convert angle to +pi to -pi return atan2(sin(data), cos(data)) def _step_angle(self, action=0.0): # target is absolute orientation wrt to current orientation # target is in radian target = action init_pose = self.bot_base.state.theta init_err = self._norm_pose(target) target_world = self._norm_pose(target + init_pose) cmd_vel = 0 min_vel = 0 got_min_vel = False prev_time = time.time() self.bot_base.should_stop = False ret_val = True while True: if time.time() - prev_time > (1.0 / self.hz): cur_error = self._norm_pose(target_world - self.bot_base.state.theta) if self.bot_base.should_stop: if not self.ignore_collisions: rospy.loginfo("curr error = {} degrees".format( degrees(cur_error))) self.stop() ret_val = False break # stop if error goes beyond some value if abs(cur_error) < self.rot_error_thr: rospy.loginfo("Reached goal") rospy.loginfo("curr_error = {} degrees".format( degrees(cur_error))) self._cmd_vel(rot_vel=0.0) break # for getting the min velocity at wchich bot starts to move if not (got_min_vel) and abs(cur_error - init_err) > self.rot_move_thr: got_min_vel = True min_vel = abs(cmd_vel) # doing the linear increse part if init_err != 0 and cur_error / init_err > 0.5: if abs(cmd_vel) < self.rot_max_vel: cmd_vel += sign(cur_error) * self.vel_delta # elif abs(cur_error) < self.drop_ang: else: if abs(cmd_vel) > 0.75 * min_vel: # 0.75 as I found min velocity is always above the # actual min required velocity cmd_vel -= sign(cur_error) * self.vel_delta # change the sign of init error if it misses if abs(cur_error) > abs(init_err): init_err = cur_error # chnage the init error if you overshooot if cur_error * init_err < 0: cmd_vel = cmd_vel / 10 init_err = cur_error self._cmd_vel(rot_vel=cmd_vel) prev_time = time.time() rospy.sleep(SLEEP_TIME) self.bot_base.should_stop = False return ret_val def _step_x(self, action=0.0): # target is the distance in x direction that robot has to move # target is in meter(only works for positive ) target = action init_x = self.bot_base.state.x init_y = self.bot_base.state.y init_err = abs(target) cmd_vel = 0 min_vel = 0 got_min_vel = False prev_time = time.time() self.bot_base.should_stop = False ret_val = True while True: if time.time() - prev_time > (1.0 / self.hz): cur_error = abs( abs(target) - sqrt((self.bot_base.state.x - init_x)**2 + (self.bot_base.state.y - init_y)**2)) if self.bot_base.should_stop: if not self.ignore_collisions: rospy.loginfo( "curr error = {} meters".format(cur_error)) self.stop() ret_val = False break # stop if error goes beyond some value if abs(cur_error) < self.dist_error_thr: rospy.loginfo("Reached goal") rospy.loginfo("curr error = {} meters".format(cur_error)) self._cmd_vel(lin_vel=0.0) break # for getting the min velocity at wchich bot starts to move if not (got_min_vel) and abs(cur_error - init_err) > self.lin_move_thr: got_min_vel = True min_vel = abs(cmd_vel) # rospy.loginfo("min vel = ", min_vel) # doing the linear increse part if cur_error / init_err > 0.6: if abs(cmd_vel) < self.lin_max_vel: cmd_vel += sign(target) * self.vel_delta # elif abs(cur_error) < self.drop_ang: else: if abs(cmd_vel) > 0.75 * min_vel: cmd_vel -= sign(target) * self.vel_delta # change the sign of init error if it misses if abs(cur_error) > abs(init_err): init_err = cur_error target = sign(target) * cur_error # chnage the init error if you overshooot if cur_error * init_err < 0: cmd_vel = cmd_vel / 10 init_err = cur_error target = -sign(target) * cur_error self._cmd_vel(lin_vel=cmd_vel) prev_time = time.time() rospy.sleep(SLEEP_TIME) self.bot_base.should_stop = False return ret_val def goto(self, xyt_position=None): """ Moves the robot to the robot to given goal state in the relative frame (base frame). :param xyt_position: The goal state of the form (x,y,t) in the relative (base) frame. :type xyt_position: list """ if xyt_position is None: xyt_position = [0.0, 0.0, 0.0] rospy.loginfo("BASE goto, cmd: {}".format(xyt_position)) x = xyt_position[0] y = xyt_position[1] rot = xyt_position[2] if sqrt(x * x + y * y) < self.translation_treshold: self._step_angle(rot) return True theta_1 = atan2(y, x) dist = sqrt(x**2 + y**2) if theta_1 > pi / 2: theta_1 = theta_1 - pi dist = -dist if theta_1 < -pi / 2: theta_1 = theta_1 + pi dist = -dist theta_2 = -theta_1 + rot # first rotate by theta1 self._step_angle(theta_1) # move the distance self._step_x(dist) # second rotate by theta2 self._step_angle(theta_2) return True def _get_xyt(self, pose): """Processes the pose message to get (x,y,theta)""" orientation_list = [ pose.pose.orientation.x, pose.pose.orientation.y, pose.pose.orientation.z, pose.pose.orientation.w, ] (roll, pitch, yaw) = euler_from_quaternion(orientation_list) goal_position = [pose.pose.position.x, pose.pose.position.y, yaw] return goal_position def go_to_absolute(self, xyt_position, close_loop=True, smooth=False): """ Moves the robot to the robot to given goal state in the world frame using proportional control. :param xyt_position: The goal state of the form (x,y,t) in the world (map) frame. :param close_loop: When set to "True", ensures that controler is operating in open loop by taking account of odometry. :param smooth: When set to "True", ensures that the motion leading to the goal is a smooth one. :type xyt_position: list or np.ndarray :type close_loop: bool :type smooth: bool """ assert (not smooth), "Proportional controller \ cannot generate smooth motion" assert (close_loop), "Proportional controller \ cannot work in open loop" pose_stamped = build_pose_msg(xyt_position[0], xyt_position[1], xyt_position[2], self.MAP_FRAME) base_pose = self._transform_listener.transformPose( self.BASE_FRAME, pose_stamped) return self.goto(self._get_xyt(base_pose))
class ServoingServer(object): def __init__(self): rospy.init_node('relative_servoing') rospy.Subscriber('robot_pose_ekf/odom_combined', PoseWithCovarianceStamped, self.update_position) #rospy.Subscriber('/base_scan', LaserScan, self.base_laser_cb) self.servoing_as = actionlib.SimpleActionServer('servoing_action', ServoAction, self.goal_cb, False) self.vel_out = rospy.Publisher('base_controller/command', Twist) self.tfl = TransformListener() self.goal_out = rospy.Publisher('/servo_dest', PoseStamped, latch=True) self.left_out = rospy.Publisher('/left_points', PointCloud) self.right_out = rospy.Publisher('/right_points', PointCloud) self.front_out = rospy.Publisher('/front_points', PointCloud) #Initialize variables, so as not to spew errors before seeing a goal self.at_goal = False self.rot_safe = True self.bfp_goal = PoseStamped() self.odom_goal = PoseStamped() self.x_max = 0.5 self.x_min = 0.05 self.y_man = 0.3 self.y_min = 0.05 self.z_max = 0.5 self.z_min = 0.05 self.ang_goal = 0.0 self.ang_thresh_small = 0.01 self.ang_thresh_big = 0.04 self.ang_thresh = self.ang_thresh_big self.retreat_thresh = 0.3 self.curr_pos = PoseWithCovarianceStamped() self.dist_thresh = 0.15 self.left = [[],[]] self.right = [[],[]] self.front = [[],[]] self.servoing_as.start() def goal_cb(self, goal): self.update_goal(goal.goal) update_rate = rospy.Rate(40) command = Twist() while not (rospy.is_shutdown() or self.at_goal): command.linear.x = self.get_trans_x() command.linear.y = self.get_trans_y() command.angular.z = self.get_rot() (x,y,z) = self.avoid_obstacles() if x is not None: command.linear.x = x if y is not None: command.linear.y = y command.angular.z += z if command.linear.y > 0: if not self.left_clear(): command.linear.y = 0.0 elif command.linear.y < 0: if not self.right_clear(): command.linear.y = 0.0 #print "Sending vel_command: \r\n %s" %self.command self.vel_out.publish(command) rospy.sleep(0.01) #Min sleep update_rate.sleep() #keep pace if self.at_goal: print "Arrived at goal" result = ServoResult() result.arrived = Bool(True) self.servoing_as.set_succeeded(result) def update_goal(self, msg): msg.header.stamp = rospy.Time.now() if not self.tfl.waitForTransform(msg.header.frame_id, '/base_footprint', msg.header.stamp, rospy.Duration(30)): rospy.logwarn('Cannot find /base_footprint transform') self.bfp_goal = self.tfl.transformPose('/base_footprint', msg) if not self.tfl.waitForTransform(msg.header.frame_id, 'odom_combined', msg.header.stamp, rospy.Duration(30)): rospy.logwarn('Cannot find /odom_combined transform') self.odom_goal = self.tfl.transformPose('/odom_combined', msg) self.goal_out.publish(self.odom_goal) ang_to_goal = math.atan2(self.bfp_goal.pose.position.y, self.bfp_goal.pose.position.x) #(current angle in odom, plus the robot-relative change to face goal) self.ang_goal = self.curr_ang[2] + ang_to_goal rospy.logwarn(self.odom_goal) rospy.logwarn(self.ang_goal) print "New Goal: \r\n %s" %self.bfp_goal def update_position(self, msg): if not self.servoing_as.is_active(): return self.curr_ang=tft.euler_from_quaternion([msg.pose.pose.orientation.x, msg.pose.pose.orientation.y, msg.pose.pose.orientation.z, msg.pose.pose.orientation.w]) # Normalized via unwrap relative to 0; (keeps between -pi/pi) self.ang_diff = np.unwrap([0, self.ang_goal - self.curr_ang[2]])[1] #print "Ang Diff: %s" %self.ang_diff self.dist_to_goal = ((self.odom_goal.pose.position.x- msg.pose.pose.position.x)**2 + (self.odom_goal.pose.position.y- msg.pose.pose.position.y)**2)**(1./2) rospy.logwarn('Distance to goal (msg): ') rospy.logwarn(msg) rospy.logwarn('Distance to goal (odom_goal): ') rospy.logwarn(self.odom_goal) if ((self.dist_to_goal < self.dist_thresh) and (abs(self.ang_diff) < self.ang_thresh)): self.at_goal = True else: self.at_goal = False def base_laser_cb(self, msg): max_angle = msg.angle_max ranges = np.array(msg.ranges) angles = np.arange(msg.angle_min, msg.angle_max, msg.angle_increment) #Filter out noise(<0.003), points >1m, leaves obstacles near_angles = np.extract(np.logical_and(ranges<1, ranges>0.003), angles) near_ranges = np.extract(np.logical_and(ranges<1, ranges>0.003), ranges) self.bad_side = np.sign(near_angles[np.argmax(abs(near_angles))]) #print "bad side: %s" %bad_side # (1 (pos) = left, -1 = right) #print "Min in Ranges: %s" %min(ranges) #if len(near_ranges) > 0: xs = near_ranges * np.cos(near_angles) ys = near_ranges * np.sin(near_angles) #print "xs: %s" %xs self.points = np.vstack((xs,ys)) #print "Points: %s" %points self.bfp_points = np.vstack((np.add(0.275, xs),ys)) #print "bfp Points: %s" %bfp_points self.bfp_dists = np.sqrt(np.add(np.square(self.bfp_points[0][:]), np.square(self.bfp_points[1][:]))) #print min(self.bfp_dists) if len(self.bfp_dists) >0: if min(self.bfp_dists) > 0.5: self.rot_safe = True else: self.rot_safe = False else: self.rot_safe = True self.left = np.vstack((xs[np.nonzero(ys>0.35)[0]], ys[np.nonzero(ys>0.35)[0]])) self.right= np.vstack((xs[np.nonzero(ys<-0.35)[0]], ys[np.nonzero(ys<-0.35)[0]])) self.front = np.vstack( (np.extract(np.logical_and(ys<0.35,ys>-0.35),xs), np.extract(np.logical_and(ys<0.35, ys>-0.35),ys))) front_dist = (self.front[:][0]**2+self.front[:][1]**2)**(1/2) ##Testing and Visualization:### if len(self.left[:][0]) > 0: leftScan = PointCloud() leftScan.header.frame_id = '/base_laser_link' leftScan.header.stamp = rospy.Time.now() for i in range(len(self.left[0][:])): pt = Point32() pt.x = self.left[0][i] pt.y = self.left[1][i] pt.z = 0 leftScan.points.append(pt) self.left_out.publish(leftScan) if len(self.right[:][0]) > 0: rightScan = PointCloud() rightScan.header.frame_id = '/base_laser_link' rightScan.header.stamp = rospy.Time.now() for i in range(len(self.right[:][0])): pt = Point32() pt.x = self.right[0][i] pt.y = self.right[1][i] pt.z = 0 rightScan.points.append(pt) self.right_out.publish(rightScan) if len(self.front[:][0]) > 0: frontScan = PointCloud() frontScan.header.frame_id = 'base_laser_link' frontScan.header.stamp = rospy.Time.now() for i in range(len(self.front[:][0])): pt = Point32() pt.x = self.front[0][i] pt.y = self.front[1][i] pt.z = 0 frontScan.points.append(pt) self.front_out.publish(frontScan) def get_rot(self): if abs(self.ang_diff) < self.ang_thresh: self.ang_thresh = self.ang_thresh_big return 0.0 else: self.ang_thresh = self.ang_thresh_small if self.rot_safe: return np.sign(self.ang_diff)*np.clip(abs(0.35*self.ang_diff), 0.1, 0.5) else: fdbk = ServoFeedback() fdbk.current_action = String("Cannot Rotate, obstacles nearby") self.servoing_as.publish_feedback(fdbk) return 0.0 def get_trans_x(self): if (abs(self.ang_diff) < math.pi/6 and self.dist_to_goal > self.dist_thresh): return np.clip(self.dist_to_goal*0.125, 0.05, 0.3) else: return 0.0 def get_trans_y(self): # Determine left/right movement speed for strafing obstacle avoidance push_from_left = push_from_right = 0.0 if len(self.left[:][0]) > 0: lefts = np.extract(self.left[:][1]<0.45, self.left[:][1]) if len(lefts) > 0: push_from_left = -0.45 + min(lefts) if len(self.right[:][0]) > 0: rights = np.extract(self.right[:][1]>-0.45, self.right[:][1]) if len(rights) > 0: push_from_right = 0.45 + max(rights) slide = push_from_right + push_from_left #print "Slide speed (m/s): %s" %slide return np.sign(slide)*np.clip(abs(slide), 0.04, 0.07) def avoid_obstacles(self): ##Determine rotation to avoid obstacles in front of robot# x = y = None z = 0. if len(self.front[0][:]) > 0: if min(self.front[0][:]) < self.retreat_thresh: #(round-up on corner-to-corner radius of robot) - # 0.275 (x diff from base laser link to base footprint) #print "front[0][:] %s" %self.front[0][:] front_dists = np.sqrt(np.add(np.square(self.front[0][:]), np.square(self.front[1][:]))) min_x = self.front[0][np.argmin(front_dists)] min_y = self.front[1][np.argmin(front_dists)] #print "min x/y: %s,%s" %(min_x, min_y) x = -np.sign(min_x)*np.clip(abs(min_x),0.05,0.1) y = -np.sign(min_y)*np.clip(abs(min_y),0.05,0.1) z = 0. # This should probably be avoided... fdbk = ServoFeedback() fdbk.current_action = String("TOO CLOSE: Back up slowly...") self.servoing_as.publish_feedback(fdbk) self.retreat_thresh = 0.4 elif min(self.front[0][:]) < 0.45: self.retreat_thresh = 0.3 fdbk = ServoFeedback() fdbk.current_action=String("Turning Away from obstacles in front") self.servoing_as.publish_feedback(fdbk) lfobs = self.front[0][np.logical_and(self.front[1]>0, self.front[0]<0.45)] rfobs = self.front[0][np.logical_and(self.front[1]<0, self.front[0]<0.45)] if len(lfobs) == 0: y = 0.07 elif len(rfobs) == 0: y = -0.07 weight = np.reciprocal(np.sum(np.reciprocal(rfobs)) - np.sum(np.reciprocal(lfobs))) if weight > 0: z = 0.05 else: z = -0.05 else: self.retreat_thresh = 0.3 return (x,y,z) def left_clear(self): # Base Laser cannot see obstacles beside the back edge of the robot, which could cause problems, especially just after passing through doorways... if len(self.left[0][:])>0: #Find points directly to the right or left of the robot (not in front or behind) # x<0.1 (not in front), x>-0.8 (not behind) left_obs = self.left[:, self.left[1,:]<0.4] #points too close. if len(left_obs[0][:])>0: left_obs = left_obs[:, np.logical_and(left_obs[0,:]<0.1, left_obs[0,:]>-0.8)] if len(left_obs[:][0])>0: fdbk = ServoFeedback() fdbk.current_action = String("Obstacle to the left, cannot move.") self.servoing_as.publish_feedback(fdbk) return False return True def right_clear(self): if len(self.right[0][:])>0: #Find points directly to the right or left of the robot (not in front or behind) # x<0.1 (not in front), x>-0.8 (not behind) right_obs = self.right[:, self.right[1,:]>-0.4] #points too close. if len(right_obs[0][:])>0: right_obs = right_obs[:, np.logical_and(right_obs[0,:]<0.1, right_obs[0,:]>-0.8)] if len(right_obs[:][0])>0: fdbk = ServoFeedback() fdbk.current_action = String("Obstacle immediately to the right, cannot move.") self.servoing_as.publish_feedback(fdbk) return False return True
class RegistrationLoader(object): WORLD_FRAME = "odom_combined" HEAD_FRAME = "head_frame" def __init__(self): self.head_pose = None self.head_pc_reg = None self.head_frame_tf = None self.head_frame_bcast = TransformBroadcaster() self.tfl = TransformListener() self.init_reg_srv = rospy.Service("/initialize_registration", HeadRegistration, self.init_reg_cb) self.confirm_reg_srv = rospy.Service("/confirm_registration", ConfirmRegistration, self.confirm_reg_cb) self.head_registration_r = rospy.ServiceProxy("/head_registration_r", HeadRegistration) self.head_registration_l = rospy.ServiceProxy("/head_registration_l", HeadRegistration) self.feedback_pub = rospy.Publisher("/feedback", String) self.test_pose = rospy.Publisher("/test_head_pose", PoseStamped) self.reg_dir = rospy.get_param("~registration_dir", "") self.subject = rospy.get_param("~subject", None) def publish_feedback(self, msg): rospy.loginfo("[%s] %s" % (rospy.get_name(), msg)) self.feedback_pub.publish(msg) def init_reg_cb(self, req): # TODO REMOVE THIS FACE SIDE MESS self.publish_feedback("Performing Head Registration. Please Wait.") self.face_side = rospy.get_param("~face_side1", 'r') bag_str = self.reg_dir + '/' + '_'.join([self.subject, self.face_side, "head_transform"]) + ".bag" rospy.loginfo("[%s] Loading %s" %(rospy.get_name(), bag_str)) try: bag = rosbag.Bag(bag_str, 'r') for topic, msg, ts in bag.read_messages(): head_tf = msg assert (head_tf is not None), "Error reading head transform bagfile" bag.close() except Exception as e: self.publish_feedback("Registration failed: Error loading saved registration.") rospy.logerr("[%s] Cannot load registration parameters from %s:\r\n%s" % (rospy.get_name(), bag_str, e)) return (False, PoseStamped()) if self.face_side == 'r': head_registration = self.head_registration_r else: head_registration = self.head_registration_l try: rospy.loginfo("[%s] Requesting head registration for %s at pixel (%d, %d)." %(rospy.get_name(), self.subject, req.u, req.v)) self.head_pc_reg = head_registration(req.u, req.v).reg_pose if ((self.head_pc_reg.pose.position == Point(0.0, 0.0, 0.0)) and (self.head_pc_reg.pose.orientation == Quaternion(0.0, 0.0, 0.0, 1.0))): raise rospy.ServiceException("Unable to find a good match.") self.head_pc_reg = None except rospy.ServiceException as se: self.publish_feedback("Registration failed: %s" %se) return (False, PoseStamped()) pc_reg_mat = PoseConv.to_homo_mat(self.head_pc_reg) head_tf_mat = PoseConv.to_homo_mat(Transform(head_tf.transform.translation, head_tf.transform.rotation)) self.head_pose = PoseConv.to_pose_stamped_msg(pc_reg_mat**-1 * head_tf_mat) self.head_pose.header.frame_id = self.head_pc_reg.header.frame_id self.head_pose.header.stamp = self.head_pc_reg.header.stamp side = "right" if (self.face_side == 'r') else "left" self.publish_feedback("Registered head using %s cheek model, please check and confirm." %side) # rospy.loginfo('[%s] Head frame registered at:\r\n %s' %(rospy.get_name(), self.head_pose)) self.test_pose.publish(self.head_pose) return (True, self.head_pose) def confirm_reg_cb(self, req): if self.head_pose is None: raise rospy.ServiceException("Head has not been registered."); return False try: hp = copy.copy(self.head_pose) now = rospy.Time.now() + rospy.Duration(0.5) self.tfl.waitForTransform(self.WORLD_FRAME, hp.header.frame_id, now, rospy.Duration(10)) hp.header.stamp = now hp_world = self.tfl.transformPose(self.WORLD_FRAME, hp) pos = (hp_world.pose.position.x, hp_world.pose.position.y, hp_world.pose.position.z) quat = (hp_world.pose.orientation.x, hp_world.pose.orientation.y, hp_world.pose.orientation.z, hp_world.pose.orientation.w) #Temp: hack for feeding system print "Modifying head frame into upright frame" rot = np.matrix([[1 - 2*quat[1]*quat[1] - 2*quat[2]*quat[2], 2*quat[0]*quat[1] - 2*quat[2]*quat[3], 2*quat[0]*quat[2] + 2*quat[1]*quat[3]], [2*quat[0]*quat[1] + 2*quat[2]*quat[3], 1 - 2*quat[0]*quat[0] - 2*quat[2]*quat[2], 2*quat[1]*quat[2] - 2*quat[0]*quat[3]], [2*quat[0]*quat[2] - 2*quat[1]*quat[3], 2*quat[1]*quat[2] + 2*quat[0]*quat[3], 1 - 2*quat[0]*quat[0] - 2*quat[1]*quat[1]]]) rot[0,2]=rot[1,2]=0.0 rot[2,2]=1.0 rot[2,0]=rot[2,1]=0.0 print rot.shape x_norm = np.linalg.norm(rot[:,0]) rot[0,0] /= x_norm rot[1,0] /= x_norm y_norm = np.linalg.norm(rot[:,1]) rot[0,1] /= y_norm rot[1,1] /= y_norm quat = matrix_to_quaternion(rot) print "Completed to modify head frame into upright frame" self.head_frame_tf = (pos, quat) self.publish_feedback("Head registration confirmed."); return True except Exception as e: rospy.logerr("[%s] Error: %s" %(rospy.get_name(), e)) raise rospy.ServiceException("Error confirming head registration.")
class jt_task_utils: def __init__(self, tf=None): if tf is None: self.tf = TransformListener() else: self.tf = tf #### SERVICES #### rospy.loginfo("Waiting for utility_frame_services") try: rospy.wait_for_service("/l_utility_frame_update", 3.0) rospy.wait_for_service("/r_utility_frame_update", 3.0) self.update_frame = [ rospy.ServiceProxy("/l_utility_frame_update", FrameUpdate), rospy.ServiceProxy("/r_utility_frame_update", FrameUpdate), ] rospy.loginfo("Found utility_frame_services") except: rospy.logwarn("Left or Right Utility Frame Service Not available") #### Action Clients #### self.ft_move_client = actionlib.SimpleActionClient("l_cart/ft_move_action", FtMoveAction) rospy.loginfo("Waiting for l_cart/ft_move_action server") if self.ft_move_client.wait_for_server(rospy.Duration(3)): rospy.loginfo("Found l_cart/ft_move_action server") else: rospy.logwarn("Cannot find l_cart/ft_move_action server") self.ft_move_r_client = actionlib.SimpleActionClient("r_cart/ft_move_action", FtMoveAction) rospy.loginfo("Waiting for r_cart/ft_move_action server") if self.ft_move_r_client.wait_for_server(rospy.Duration(3)): rospy.loginfo("Found r_cart/ft_move_action server") else: rospy.logwarn("Cannot find r_cart/ft_move_action server") self.ft_hold_client = actionlib.SimpleActionClient("ft_hold_action", FtHoldAction) rospy.loginfo("Waiting for ft_hold_action server") if self.ft_hold_client.wait_for_server(rospy.Duration(3)): rospy.loginfo("Found ft_hold_action server") else: rospy.logwarn("Cannot find ft_hold_action server") #### SUBSCRIBERS #### self.curr_state = [PoseStamped(), PoseStamped()] rospy.Subscriber("/l_cart/state/x", PoseStamped, self.get_l_state) rospy.Subscriber("/r_cart/state/x", PoseStamped, self.get_r_state) rospy.Subscriber("/wt_l_wrist_command", Point, self.rot_l_wrist) rospy.Subscriber("/wt_r_wrist_command", Point, self.rot_r_wrist) rospy.Subscriber("/wt_left_arm_pose_commands", Point, self.trans_l_hand) rospy.Subscriber("/wt_right_arm_pose_commands", Point, self.trans_r_hand) # self.ft_wrench = WrenchStamped() # self.force_stopped = False # self.ft_z_thresh = -2 # self.ft_mag_thresh = 5 # rospy.Subscriber('ft_data_pm_adjusted', WrenchStamped, self.get_ft_state) #### PUBLISHERS #### self.goal_pub = [ rospy.Publisher("l_cart/command_pose", PoseStamped), rospy.Publisher("r_cart/command_pose", PoseStamped), ] self.posture_pub = [ rospy.Publisher("l_cart/command_posture", Float64MultiArray), rospy.Publisher("r_cart/command_posture", Float64MultiArray), ] #### STATIC DATA #### self.postures = { "off": [], "mantis": [0, 1, 0, -1, 3.14, -1, 3.14], "elbowupr": [-0.79, 0, -1.6, 9999, 9999, 9999, 9999], "elbowupl": [0.79, 0, 1.6, 9999, 9999, 9999, 9999], "old_elbowupr": [-0.79, 0, -1.6, -0.79, 3.14, -0.79, 5.49], "old_elbowupl": [0.79, 0, 1.6, -0.79, 3.14, -0.79, 5.49], "elbowdownr": [-0.028262, 1.294634, -0.2578564, -1.549888, -31.27891385, -1.05276449, -1.8127318], "elbowdownl": [-0.008819572, 1.28348282, 0.2033844, -1.5565279, -0.09634, -1.023502, 1.799089], } def get_l_state(self, ps): # WORKING, TESTED self.curr_state[0] = ps def get_r_state(self, ps): # WORKING, TESTED self.curr_state[1] = ps # def get_ft_state(self, ws): # self.ft_wrench = ws # self.ft_mag = math.sqrt(ws.wrench.force.x**2 + ws.wrench.force.y**2 + ws.wrench.force.z**2) # if ws.wrench.force.z < self.ft_z_thresh: # self.force_stopped = True # # rospy.logwarn("Z force threshold exceeded") # if self.ft_mag > self.ft_mag_thresh: # self.force_stopped = True # rospy.logwarn("Total force threshold exceeded") def rot_l_wrist(self, pt): out_pose = deepcopy(self.curr_state[0]) q_r = transformations.quaternion_about_axis(pt.x, (1, 0, 0)) # Hand frame roll (hand roll) q_p = transformations.quaternion_about_axis(pt.y, (0, 1, 0)) # Hand frame pitch (wrist flex) q_h = transformations.quaternion_multiply(q_r, q_p) q_f = transformations.quaternion_about_axis(pt.y, (1, 0, 0)) # Forearm frame rot (forearm roll) if pt.x or pt.y: self.tf.waitForTransform( out_pose.header.frame_id, "l_wrist_roll_link", out_pose.header.stamp, rospy.Duration(3.0) ) hand_pose = self.tf.transformPose("l_wrist_roll_link", out_pose) q_hand_pose = ( hand_pose.pose.orientation.x, hand_pose.pose.orientation.y, hand_pose.pose.orientation.z, hand_pose.pose.orientation.w, ) q_h_rot = transformations.quaternion_multiply(q_h, hand_pose.pose.orientation) hand_pose.pose.orientation = Quaternion(*q_h_rot) out_pose = self.tf.transformPose(out_pose.header.frame_id, hand_pose) if pt.z: self.tf.waitForTransform( out_pose.header.frame_id, "l_forearm_roll_link", out_pose.header.stamp, rospy.Duration(3.0) ) hand_pose = self.tf.transformPose("l_forearm_roll_link", out_pose) q_hand_pose = ( hand_pose.pose.orientation.x, hand_pose.pose.orientation.y, hand_pose.pose.orientation.z, hand_pose.pose.orientation.w, ) q_f_rot = transformations.quaternion_multiply(q_f, hand_pose.pose.orientation) hand_pose.pose.orientation = Quaternion(*q_f_rot) out_pose = self.tf.transformPose(out_pose.header.frame_id, hand_pose) wrist_traj = self.build_trajectory(out_pose, arm=0) # TODO: Add Action Goal Sender to Move along trajectory!!!!!!!!!!!!!!!! def rot_r_wrist(self, pt): out_pose = deepcopy(self.curr_state[1]) q_r = transformations.quaternion_about_axis(-pt.x, (1, 0, 0)) # Hand frame roll (hand roll) q_p = transformations.quaternion_about_axis(-pt.y, (0, 1, 0)) # Hand frame pitch (wrist flex) q_h = transformations.quaternion_multiply(q_r, q_p) q_f = transformations.quaternion_about_axis(-pt.y, (1, 0, 0)) # Forearm frame rot (forearm roll) if pt.x or pt.y: self.tf.waitForTransform( out_pose.header.frame_id, "r_wrist_roll_link", out_pose.header.stamp, rospy.Duration(3.0) ) hand_pose = self.tf.transformPose("r_wrist_roll_link", out_pose) q_hand_pose = ( hand_pose.pose.orientation.x, hand_pose.pose.orientation.y, hand_pose.pose.orientation.z, hand_pose.pose.orientation.w, ) q_h_rot = transformations.quaternion_multiply(q_h, hand_pose.pose.orientation) hand_pose.pose.orientation = Quaternion(*q_h_rot) out_pose = self.tf.transformPose(out_pose.header.frame_id, hand_pose) if pt.z: self.tf.waitForTransform( out_pose.header.frame_id, "r_forearm_roll_link", out_pose.header.stamp, rospy.Duration(3.0) ) hand_pose = self.tf.transformPose("r_forearm_roll_link", out_pose) q_hand_pose = ( hand_pose.pose.orientation.x, hand_pose.pose.orientation.y, hand_pose.pose.orientation.z, hand_pose.pose.orientation.w, ) q_f_rot = transformations.quaternion_multiply(q_f, hand_pose.pose.orientation) hand_pose.pose.orientation = Quaternion(*q_f_rot) out_pose = self.tf.transformPose(out_pose.header.frame_id, hand_pose) wrist_traj = self.build_trajectory(out_pose, arm=1) # TODO: Add Action Goal Sender to Move along trajectory!!!!!!!!!!!!!!!! def trans_l_hand(self, pt): print "Moving Left Hand with JT Task Controller" out_pose = PoseStamped() out_pose.header.frame_id = self.curr_state[0].header.frame_id out_pose.header.stamp = rospy.Time.now() out_pose.pose.position.x = self.curr_state[0].pose.position.x + pt.x out_pose.pose.position.y = self.curr_state[0].pose.position.y + pt.y out_pose.pose.position.z = self.curr_state[0].pose.position.z + pt.z out_pose.pose.orientation = self.curr_state[0].pose.orientation trans_traj = self.build_trajectory(out_pose, arm=0) self.ft_move_client.send_goal(FtMoveGoal(trans_traj, 0.0, True)) self.ft_move_client.wait_for_result(rospy.Duration(0.025 * len(trans_traj))) def trans_r_hand(self, pt): out_pose = PoseStamped() out_pose.header.frame_id = self.curr_state[1].header.frame_id out_pose.header.stamp = rospy.Time.now() out_pose.pose.position.x = self.curr_state[1].pose.position.x + pt.x out_pose.pose.position.y = self.curr_state[1].pose.position.y + pt.y out_pose.pose.position.z = self.curr_state[1].pose.position.z + pt.z out_pose.pose.orientation = self.curr_state[1].pose.orientation trans_traj = self.build_trajectory(out_pose, arm=0) self.ft_move_client.send_goal(FtMoveGoal(trans_traj, 0.0, True)) self.ft_move_client.wait_for_result(rospy.Duration(0.025 * len(trans_traj))) def send_posture(self, posture="off", arm=0): # WORKING, TESTED TODO: SLOW TRANSITION (if possible) if "elbow" in posture: if arm == 0: posture = posture + "l" elif arm == 1: posture = posture + "r" self.posture_pub[arm].publish(Float64MultiArray(data=self.postures[posture])) def send_traj(self, poses, arm=0): send_rate = rospy.Rate(50) ##!!!!!!!!!!!! MUST BALANCE SEND RATE WITH SPACING IN 'BUILD_TRAJECTORY' FOR CONTROL OF VELOCITY !!!!!!!!!!!! finished = False count = 0 while not (rospy.is_shutdown() or finished): self.goal_pub[arm].publish(poses[count]) count += 1 send_rate.sleep() if count == len(poses): finished = True def send_traj_to_contact(self, poses, arm=0): send_rate = rospy.Rate(20) ##!!!!!!!!!!!! MUST BALANCE SEND RATE WITH SPACING IN 'BUILD_TRAJECTORY' FOR CONTROL OF VELOCITY !!!!!!!!!!!! finished = False count = 0 while not (rospy.is_shutdown() or finished or self.force_stopped): self.goal_pub[arm].publish(poses[count]) count += 1 send_rate.sleep() if count == len(poses): finished = True def build_trajectory(self, finish, start=None, arm=0, space=0.001, steps=None): # WORKING, TESTED ##!!!!!!!!!!!! MUST BALANCE SPACING WITH SEND RATE IN 'SEND_TRAJ' FOR CONTROL OF VELOCITY !!!!!!!!!!!! if start is None: # if given one pose, use current position as start start = self.curr_state[arm] dist = self.calc_dist(start, finish, arm=arm) # Total distance to travel if steps is None: steps = int(math.ceil(dist / space)) fracs = np.linspace(0, 1, steps) # A list of fractional positions along course print "Steps: %s" % steps poses = [PoseStamped() for i in xrange(steps)] xs = np.linspace(start.pose.position.x, finish.pose.position.x, steps) ys = np.linspace(start.pose.position.y, finish.pose.position.y, steps) zs = np.linspace(start.pose.position.z, finish.pose.position.z, steps) qs = [start.pose.orientation.x, start.pose.orientation.y, start.pose.orientation.z, start.pose.orientation.w] qf = [ finish.pose.orientation.x, finish.pose.orientation.y, finish.pose.orientation.z, finish.pose.orientation.w, ] for i, frac in enumerate(fracs): poses[i].header.stamp = rospy.Time.now() poses[i].header.frame_id = start.header.frame_id poses[i].pose.position = Point(xs[i], ys[i], zs[i]) new_q = transformations.quaternion_slerp(qs, qf, frac) poses[i].pose.orientation = Quaternion(*new_q) # rospy.loginfo("Planning straight-line path, please wait") # self.wt_log_out.publish(data="Planning straight-line path, please wait") return poses def pose_frame_move(self, pose, x, y=0, z=0, arm=0): # FINISHED, UNTESTED self.update_frame[arm](pose) if arm == 0: frame = "lh_utility_frame" elif arm == 1: frame = "rh_utility_frame" pose.header.stamp = rospy.Time.now() self.tf.waitForTransform(pose.header.frame_id, frame, pose.header.stamp, rospy.Duration(3.0)) framepose = self.tf.transformPose(frame, pose) framepose.pose.position.x += x framepose.pose.position.y += y framepose.pose.position.z += z self.dist = math.sqrt(x ** 2 + y ** 2 + z ** 2) self.tf.waitForTransform(frame, pose.header.frame_id, pose.header.stamp, rospy.Duration(3.0)) return self.tf.transformPose(pose.header.frame_id, framepose) def calc_dist(self, ps1, ps2=None, arm=0): # FINISHED, UNTESTED if ps2 is None: ps2 = self.curr_pose() p1 = ps1.pose.position p2 = ps2.pose.position wrist_dist = math.sqrt((p1.x - p2.x) ** 2 + (p1.y - p2.y) ** 2 + (p1.z - p2.z) ** 2) self.update_frame[arm](ps2) ps2.header.stamp = rospy.Time(0) np2 = self.tf.transformPose("lh_utility_frame", ps2) np2.pose.position.x += 0.21 self.tf.waitForTransform(np2.header.frame_id, "torso_lift_link", rospy.Time.now(), rospy.Duration(3.0)) p2 = self.tf.transformPose("torso_lift_link", np2) self.update_frame[arm](ps1) ps1.header.stamp = rospy.Time(0) np1 = self.tf.transformPose("lh_utility_frame", ps1) np1.pose.position.x += 0.21 self.tf.waitForTransform(np1.header.frame_id, "torso_lift_link", rospy.Time.now(), rospy.Duration(3.0)) p1 = self.tf.transformPose("torso_lift_link", np1) p1 = p1.pose.position p2 = p2.pose.position finger_dist = math.sqrt((p1.x - p2.x) ** 2 + (p1.y - p2.y) ** 2 + (p1.z - p2.z) ** 2) dist = max(wrist_dist, finger_dist) print "Calculated Distance: ", dist return dist def test(self): print "Testing..." rospy.sleep(1) #### TEST STATE GRABBING #### print "Left Current Pose:" print self.curr_state[0] print "Right Current Pose:" print self.curr_state[1] #### TEST FORCE STATE GRABBING #### print "Current Force Wrench:" print self.ft_wrench print "Current Force Magnitude:" print self.ft_mag #### TEST LEFT ARM GOAL SENDING #### l_pose = PoseStamped() l_pose.header.frame_id = "torso_lift_link" l_pose.pose.position = Point(0.6, 0.3, 0.1) l_pose.pose.orientation = Quaternion(1, 0, 0, 0) raw_input("send left arm goal") self.goal_pub[0].publish(l_pose) #### TEST RIGHT ARM GOAL SENDING # r_pose = PoseStamped() # r_pose.header.frame_id = 'torso_lift_link' # r_pose.pose.position = Point(0.6, -0.3, 0.1) # r_pose.pose.orientation = Quaternion(1,0,0,0) # raw_input("send right arm goal") # self.goal_pub[1].publish(r_pose) #### TEST POSE SETTING #### # raw_input("Left Elbow Up") # self.send_posture('elbowup',0) # raw_input("Right Elbow Up") # self.send_posture('elbowup',1) # raw_input("Left Elbow Down") # self.send_posture('elbowdown',0) # raw_input("Right Elbow Down") # self.send_posture('elbowdown',1) # raw_input("Both Postures Off") # self.send_posture(arm=0) # self.send_posture(arm=1) # print "Postures adjusted" #### TEST TRAJECTORY MOTION #### l_pose2 = PoseStamped() l_pose2.header.frame_id = "torso_lift_link" l_pose2.pose.position = Point(0.8, 0.3, 0.1) l_pose2.pose.orientation = Quaternion(1, 0, 0, 0) raw_input("Left trajectory") # l_pose2 = self.pose_frame_move(self.curr_state[0], -0.1, arm=0) traj = self.build_trajectory(l_pose2) self.send_traj_to_contact(traj) # r_pose2 = PoseStamped() # r_pose2.header.frame_id = 'torso_lift_link' # r_pose2.pose.position = Point(0.8, -0.15, -0.3) # r_pose2.pose.orientation = Quaternion(0,0.5,0.5,0) # raw_input("Right trajectory") # r_pose2 = self.pose_frame_move(self.curr_state[1], -0.1, arm=1) # traj = self.build_trajectory(r_pose2, arm=1) # self.send_traj(traj,1) #### RECONFIRM POSITION #### print "New Left Pose:" print self.curr_state[0] print "New Right Pose:" print self.curr_state[1]
class ParticleFilter: """ The class that represents a Particle Filter ROS Node Attributes list: initialized: a Boolean flag to communicate to other class methods that initializaiton is complete base_frame: the name of the robot base coordinate frame (should be "base_link" for most robots) map_frame: the name of the map coordinate frame (should be "map" in most cases) odom_frame: the name of the odometry coordinate frame (should be "odom" in most cases) scan_topic: the name of the scan topic to listen to (should be "scan" in most cases) n_particles: the number of particles in the filter d_thresh: the amount of linear movement before triggering a filter update a_thresh: the amount of angular movement before triggering a filter update laser_max_distance: the maximum distance to an obstacle we should use in a likelihood calculation pose_listener: a subscriber that listens for new approximate pose estimates (i.e. generated through the rviz GUI) particle_pub: a publisher for the particle cloud laser_subscriber: listens for new scan data on topic self.scan_topic tf_listener: listener for coordinate transforms tf_broadcaster: broadcaster for coordinate transforms particle_cloud: a list of particles representing a probability distribution over robot poses current_odom_xy_theta: the pose of the robot in the odometry frame when the last filter update was performed. The pose is expressed as a list [x,y,theta] (where theta is the yaw) map: the map we will be localizing ourselves in. The map should be of type nav_msgs/OccupancyGrid """ def __init__(self): self.initialized = False # make sure we don't perform updates before everything is setup rospy.init_node( 'pf') # tell roscore that we are creating a new node named "pf" self.base_frame = "base_link" # the frame of the robot base self.map_frame = "map" # the name of the map coordinate frame self.odom_frame = "odom" # the name of the odometry coordinate frame self.scan_topic = "scan" # the topic where we will get laser scans from self.n_particles = 300 # the number of particles to use self.d_thresh = 0.2 # the amount of linear movement before performing an update self.a_thresh = math.pi / 6 # the amount of angular movement before performing an update self.laser_max_distance = 2.0 # maximum penalty to assess in the likelihood field model # TODO: define additional constants if needed # Setup pubs and subs # pose_listener responds to selection of a new approximate robot location (for instance using rviz) self.pose_listener = rospy.Subscriber("initialpose", PoseWithCovarianceStamped, self.update_initial_pose) # publish the current particle cloud. This enables viewing particles in rviz. self.particle_pub = rospy.Publisher("particlecloud", PoseArray) # laser_subscriber listens for data from the lidar self.laser_subscriber = rospy.Subscriber(self.scan_topic, LaserScan, self.scan_received) # enable listening for and broadcasting coordinate transforms self.tf_listener = TransformListener() self.tf_broadcaster = TransformBroadcaster() self.particle_cloud = [] self.current_odom_xy_theta = [] # request the map from the map server, the map should be of type nav_msgs/OccupancyGrid # TODO: fill in the appropriate service call here. The resultant map should be assigned be passed # into the init method for OccupancyField # for now we have commented out the occupancy field initialization until you can successfully fetch the map #self.occupancy_field = OccupancyField(map) self.initialized = True def update_robot_pose(self): """ Update the estimate of the robot's pose given the updated particles. There are two logical methods for this: (1): compute the mean pose (level 2) (2): compute the most likely pose (i.e. the mode of the distribution) (level 1) """ # first make sure that the particle weights are normalized self.normalize_particles() # TODO: assign the lastest pose into self.robot_pose as a geometry_msgs.Pose object # just to get started we will fix the robot's pose to always be at the origin self.robot_pose = Pose() def update_particles_with_odom(self, msg): """ Update the particles using the newly given odometry pose. The function computes the value delta which is a tuple (x,y,theta) that indicates the change in position and angle between the odometry when the particles were last updated and the current odometry. msg: this is not really needed to implement this, but is here just in case. """ new_odom_xy_theta = TransformHelpers.convert_pose_to_xy_and_theta( self.odom_pose.pose) # compute the change in x,y,theta since our last update if self.current_odom_xy_theta: old_odom_xy_theta = self.current_odom_xy_theta delta = (new_odom_xy_theta[0] - self.current_odom_xy_theta[0], new_odom_xy_theta[1] - self.current_odom_xy_theta[1], new_odom_xy_theta[2] - self.current_odom_xy_theta[2]) self.current_odom_xy_theta = new_odom_xy_theta else: self.current_odom_xy_theta = new_odom_xy_theta return # TODO: modify particles using delta # For added difficulty: Implement sample_motion_odometry (Prob Rob p 136) def map_calc_range(self, x, y, theta): """ Difficulty Level 3: implement a ray tracing likelihood model... Let me know if you are interested """ # TODO: nothing unless you want to try this alternate likelihood model pass def resample_particles(self): """ Resample the particles according to the new particle weights. The weights stored with each particle should define the probability that a particular particle is selected in the resampling step. You may want to make use of the given helper function draw_random_sample. """ # make sure the distribution is normalized self.normalize_particles() # TODO: fill out the rest of the implementation def update_particles_with_laser(self, msg): """ Updates the particle weights in response to the scan contained in the msg """ # TODO: implement this pass @staticmethod def angle_normalize(z): """ convenience function to map an angle to the range [-pi,pi] """ return math.atan2(math.sin(z), math.cos(z)) @staticmethod def angle_diff(a, b): """ Calculates the difference between angle a and angle b (both should be in radians) the difference is always based on the closest rotation from angle a to angle b examples: angle_diff(.1,.2) -> -.1 angle_diff(.1, 2*math.pi - .1) -> .2 angle_diff(.1, .2+2*math.pi) -> -.1 """ a = ParticleFilter.angle_normalize(a) b = ParticleFilter.angle_normalize(b) d1 = a - b d2 = 2 * math.pi - math.fabs(d1) if d1 > 0: d2 *= -1.0 if math.fabs(d1) < math.fabs(d2): return d1 else: return d2 @staticmethod def weighted_values(values, probabilities, size): """ Return a random sample of size elements from the set values with the specified probabilities values: the values to sample from (numpy.ndarray) probabilities: the probability of selecting each element in values (numpy.ndarray) size: the number of samples """ bins = np.add.accumulate(probabilities) return values[np.digitize(random_sample(size), bins)] @staticmethod def draw_random_sample(choices, probabilities, n): """ Return a random sample of n elements from the set choices with the specified probabilities choices: the values to sample from represented as a list probabilities: the probability of selecting each element in choices represented as a list n: the number of samples """ values = np.array(range(len(choices))) probs = np.array(probabilities) bins = np.add.accumulate(probs) inds = values[np.digitize(random_sample(n), bins)] samples = [] for i in inds: samples.append(deepcopy(choices[int(i)])) return samples def update_initial_pose(self, msg): """ Callback function to handle re-initializing the particle filter based on a pose estimate. These pose estimates could be generated by another ROS Node or could come from the rviz GUI """ xy_theta = TransformHelpers.convert_pose_to_xy_and_theta(msg.pose.pose) self.initialize_particle_cloud(xy_theta) self.fix_map_to_odom_transform(msg) def initialize_particle_cloud(self, xy_theta=None): """ Initialize the particle cloud. Arguments xy_theta: a triple consisting of the mean x, y, and theta (yaw) to initialize the particle cloud around. If this input is ommitted, the odometry will be used """ if xy_theta == None: xy_theta = TransformHelpers.convert_pose_to_xy_and_theta( self.odom_pose.pose) self.particle_cloud = [] self.particle_cloud.append(Particle(0, 0, 0)) # TODO create particles self.normalize_particles() self.update_robot_pose() def normalize_particles(self): """ Make sure the particle weights define a valid distribution (i.e. sum to 1.0) """ # TODO: implement this def publish_particles(self, msg): particles_conv = [] for p in self.particle_cloud: particles_conv.append(p.as_pose()) # actually send the message so that we can view it in rviz self.particle_pub.publish( PoseArray(header=Header(stamp=rospy.Time.now(), frame_id=self.map_frame), poses=particles_conv)) def scan_received(self, msg): """ This is the default logic for what to do when processing scan data. Feel free to modify this, however, I hope it will provide a good guide. The input msg is an object of type sensor_msgs/LaserScan """ if not (self.initialized): # wait for initialization to complete return if not (self.tf_listener.canTransform( self.base_frame, msg.header.frame_id, msg.header.stamp)): # need to know how to transform the laser to the base frame # this will be given by either Gazebo or neato_node return if not (self.tf_listener.canTransform(self.base_frame, self.odom_frame, msg.header.stamp)): # need to know how to transform between base and odometric frames # this will eventually be published by either Gazebo or neato_node return # calculate pose of laser relative ot the robot base p = PoseStamped( header=Header(stamp=rospy.Time(0), frame_id=msg.header.frame_id)) self.laser_pose = self.tf_listener.transformPose(self.base_frame, p) # find out where the robot thinks it is based on its odometry p = PoseStamped(header=Header(stamp=msg.header.stamp, frame_id=self.base_frame), pose=Pose()) self.odom_pose = self.tf_listener.transformPose(self.odom_frame, p) # store the the odometry pose in a more convenient format (x,y,theta) new_odom_xy_theta = TransformHelpers.convert_pose_to_xy_and_theta( self.odom_pose.pose) if not (self.particle_cloud): # now that we have all of the necessary transforms we can update the particle cloud self.initialize_particle_cloud() # cache the last odometric pose so we can only update our particle filter if we move more than self.d_thresh or self.a_thresh self.current_odom_xy_theta = new_odom_xy_theta # update our map to odom transform now that the particles are initialized self.fix_map_to_odom_transform(msg) elif (math.fabs(new_odom_xy_theta[0] - self.current_odom_xy_theta[0]) > self.d_thresh or math.fabs(new_odom_xy_theta[1] - self.current_odom_xy_theta[1]) > self.d_thresh or math.fabs(new_odom_xy_theta[2] - self.current_odom_xy_theta[2]) > self.a_thresh): # we have moved far enough to do an update! self.update_particles_with_odom(msg) # update based on odometry self.update_particles_with_laser(msg) # update based on laser scan self.update_robot_pose() # update robot's pose self.resample_particles( ) # resample particles to focus on areas of high density self.fix_map_to_odom_transform( msg ) # update map to odom transform now that we have new particles # publish particles (so things like rviz can see them) self.publish_particles(msg) def fix_map_to_odom_transform(self, msg): """ Super tricky code to properly update map to odom transform... do not modify this... Difficulty level infinity. """ (translation, rotation) = TransformHelpers.convert_pose_inverse_transform( self.robot_pose) p = PoseStamped( pose=TransformHelpers.convert_translation_rotation_to_pose( translation, rotation), header=Header(stamp=msg.header.stamp, frame_id=self.base_frame)) self.odom_to_map = self.tf_listener.transformPose(self.odom_frame, p) (self.translation, self.rotation) = TransformHelpers.convert_pose_inverse_transform( self.odom_to_map.pose) def broadcast_last_transform(self): """ Make sure that we are always broadcasting the last map to odom transformation. This is necessary so things like move_base can work properly. """ if not (hasattr(self, 'translation') and hasattr(self, 'rotation')): return self.tf_broadcaster.sendTransform(self.translation, self.rotation, rospy.get_rostime(), self.odom_frame, self.map_frame)