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)
