class Right_Utility_Frame():
frame = 'base_footprint'
px = py = pz = 0
qx = qy = qz = 0
qw = 1
def __init__(self):
rospy.init_node('right_utilitiy_frame_source')
self.updater = rospy.Service('r_utility_frame_update', FrameUpdate,
self.update_frame)
self.tfb = TransformBroadcaster()
def update_frame(self, req):
ps = req.pose
self.frame = ps.header.frame_id
self.px = ps.pose.position.x
self.py = ps.pose.position.y
self.pz = ps.pose.position.z
self.qx = ps.pose.orientation.x
self.qy = ps.pose.orientation.y
self.qz = ps.pose.orientation.z
self.qw = ps.pose.orientation.w
self.tfb.sendTransform(
(self.px, self.py, self.pz), (self.qx, self.qy, self.qz, self.qw),
rospy.Time.now(), "rh_utility_frame", self.frame)
rsp = Bool()
rsp.data = True
return rsp
class Right_Utility_Frame():
frame = 'base_footprint'
px = py = pz = 0;
qx = qy = qz = 0;
qw = 1;
def __init__(self):
rospy.init_node('right_utilitiy_frame_source')
self.updater = rospy.Service('r_utility_frame_update', FrameUpdate, self.update_frame)
self.tfb = TransformBroadcaster()
def update_frame(self, req):
ps = req.pose
self.frame = ps.header.frame_id
self.px = ps.pose.position.x
self.py = ps.pose.position.y
self.pz = ps.pose.position.z
self.qx = ps.pose.orientation.x
self.qy = ps.pose.orientation.y
self.qz = ps.pose.orientation.z
self.qw = ps.pose.orientation.w
self.tfb.sendTransform((self.px,self.py,self.pz),(self.qx,self.qy,self.qz,self.qw), rospy.Time.now(), "rh_utility_frame", self.frame)
rsp = Bool()
rsp.data = True
return rsp
def echo_socket(ws):
b = TransformBroadcaster()
f = open("orientation.txt", "a")
while True:
message = ws.receive()
orient_data = message.split(',')
orient_data = [float(data) for data in orient_data]
stamped_msg = SensorMsgStamped()
stamped_msg.data = orient_data
stamped_msg.header.stamp.secs = Time.now().secs
stamped_msg.header.stamp.nsecs = Time.now().nsecs
orient_pub_stamped.publish(stamped_msg)
### Publish to Pose topic for visualization ###
q = quaternion_from_euler(orient_data[1], orient_data[2],
orient_data[3])
pose_msg = Pose()
pose_msg.orientation.x = q[0]
pose_msg.orientation.y = q[1]
pose_msg.orientation.z = q[2]
pose_msg.orientation.w = q[3]
pose_pub.publish(pose_msg)
b.sendTransform((1, 1, 1), (q[0], q[1], q[2], q[3]), Time.now(),
'child_link', 'base_link')
### END HERE ###
print("[INFO:] Orientation{}".format(orient_data))
ws.send(message)
print >> f, message
f.close()
class Left_Utility_Frame():
frame = 'base_footprint'
px = py = pz = 0;
qx = qy = qz = 0;
qw = 1;
def __init__(self):
rospy.init_node('left_utilitiy_frame_source')
self.updater = rospy.Service('l_utility_frame_update', FrameUpdate, self.update_frame)
self.tfb = TransformBroadcaster()
def update_frame(self, req):
ps = req.pose
if not (math.isnan(ps.pose.orientation.x) or
math.isnan(ps.pose.orientation.y) or
math.isnan(ps.pose.orientation.z) or
math.isnan(ps.pose.orientation.w)):
self.frame = ps.header.frame_id
self.px = ps.pose.position.x
self.py = ps.pose.position.y
self.pz = ps.pose.position.z
self.qx = ps.pose.orientation.x
self.qy = ps.pose.orientation.y
self.qz = ps.pose.orientation.z
self.qw = ps.pose.orientation.w
else:
rospy.logerr("NAN's sent to l_utility_frame_source")
self.tfb.sendTransform((self.px,self.py,self.pz),(self.qx,self.qy,self.qz,self.qw), rospy.Time.now(), "lh_utility_frame", self.frame)
rsp = Bool()
rsp.data = True
return rsp
def main():
rospy.init_node('Odometria_Xbox')
b = TransformBroadcaster()
translation = (0.0, 0.0, 0.0)
rotation = (0.0, 0.0, 0.0, 1.0) #quaternion
rate = rospy.Rate(5) # 5hz
yant = 0.0
xant = 0.0
while not rospy.is_shutdown():
#y = math.degrees(math.asin(joy.leftX())) #+ yant
y = joy.leftX() + yant
x = joy.leftY() + xant
translation = (x, 0.0, 0.0)
rotation = tf.transformations.quaternion_from_euler(0, 0, y)
yant = y
xant = x
#print(y)
b.sendTransform(translation, rotation, Time.now(), 'camera_link',
'odom')
rate.sleep()
def handle_model_state(msg):
global last_time
br = TransformBroadcaster()
model_names = msg.name
# rospy.logerr_throttle(1,f'{str(model_names)}')
try:
# idx = model_names.index(tfPrefix)
time = rospy.Time.now()
if time == last_time:
return
idx = -1
for i in range(len(model_names)):
# rospy.logerr_throttle_identical(1,f'{tfPrefix}:{model_names[i]}')
if tfPrefix == model_names[i]:
idx = i
# rospy.logwarn('FOUND')
break
pose = msg.pose[idx]
br.sendTransform((pose.position.x, pose.position.y, pose.position.z),
(pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w),
time,
tfPrefix + "/" + "base_link",
"world")
last_time = time
except IndexError:
rospy.logwarn_throttle(10, f'Cannot find Gazebo model state {tfPrefix}')
class OdometryPublisher:
def __init__(self,
frame="odom",
child_frame="base_footprint",
queue_size=5):
self._frame_id = frame
self._child_frame_id = child_frame
self._publisher = Publisher("odom", Odometry, queue_size=queue_size)
self._broadcaster = TransformBroadcaster()
self._odom_msg = Odometry()
self._odom_msg.header.frame_id = self._frame_id
self._odom_msg.child_frame_id = self._child_frame_id
def _msg(self, now, x, y, quat, v, w):
"""
Updates the Odometry message
:param now: Current time
:param x_dist: Current x distance traveled
:param y_dist: Current y distance traveled
:param quat: Quaternion of the orientation
:param v: Linear velocity
:param w: Angular velocity
:return: Odometry message
"""
self._odom_msg.header.stamp = now
self._odom_msg.pose.pose.position.x = x
self._odom_msg.pose.pose.position.y = y
self._odom_msg.pose.pose.position.z = 0
self._odom_msg.pose.pose.orientation = quat
self._odom_msg.twist.twist = Twist(Vector3(v, 0, 0), Vector3(0, 0, w))
return self._odom_msg
def publish(self, now, x, y, v, w, heading, log=False):
"""
Publishes an Odometry message
:param cdist: Center (or average) distance of wheel base
:param v: Linear velocity
:param w: Angular velocity
:param heading: Heading (or yaw)
:param quat: Quaternion of orientation
:return: None
"""
quat = Quaternion()
quat.x = 0.0
quat.y = 0.0
quat.z = math.sin(heading / 2)
quat.w = math.cos(heading / 2)
self._broadcaster.sendTransform((x, y, 0),
(quat.x, quat.y, quat.z, quat.w), now,
self._child_frame_id, self._frame_id)
self._publisher.publish(self._msg(now, x, y, quat, v, w))
if log:
loginfo(
"Publishing Odometry: heading {:02.3f}, dist {:02.3f}, velocity {:02.3f}/{:02.3f}"
.format(heading, math.sqrt(x**2 + y**2), v, w))
def update_transform(self, pose, target_frame='base_laser_link'):
# Updates the transform between the map frame and the odom frame
br = TransformBroadcaster()
br.sendTransform((pose[0], pose[1], 0),
quaternion_from_euler(0, 0, pose[2]+math.pi),
rospy.get_rostime(),
target_frame,
'map')
class LaunchObserver(object):
"""
Keeps track of the flying/landed state of a single drone, and publishes
a tf message keeping track of the coordinates from which the drone most recently launched.
"""
def __init__(self):
self.tfl = TransformListener()
self.tfb = TransformBroadcaster()
self.flying_state_sub = rospy.Subscriber(
'states/ardrone3/PilotingState/FlyingStateChanged',
Ardrone3PilotingStateFlyingStateChanged, self.on_flying_state)
self.is_flying = True
self.RATE = 5 # republish hz
self.saved_translation = [0, 0, 0] # In meters
self.saved_yaw = 0 # In radians
self.name = rospy.get_namespace().strip('/')
self.base_link = self.name + '/base_link'
self.launch = self.name + '/launch'
self.odom = self.name + '/odom'
def on_flying_state(self, msg):
self.is_flying = msg.state in [
Ardrone3PilotingStateFlyingStateChanged.state_takingoff,
Ardrone3PilotingStateFlyingStateChanged.state_hovering,
Ardrone3PilotingStateFlyingStateChanged.state_flying,
Ardrone3PilotingStateFlyingStateChanged.state_landing,
Ardrone3PilotingStateFlyingStateChanged.state_emergency_landing
]
def spin(self):
r = rospy.Rate(self.RATE)
self.tfl.waitForTransform(self.odom, self.base_link, rospy.Time(0),
rospy.Duration.from_sec(99999))
while not rospy.is_shutdown():
if not self.is_flying:
# On the ground, update the transform
pos, quat = self.tfl.lookupTransform(self.base_link, self.odom,
rospy.Time(0))
pos[2] = 0
self.saved_translation = pos
_, _, self.saved_yaw = euler_from_quaternion(quat)
time = max(rospy.Time.now(),
self.tfl.getLatestCommonTime(
self.odom, self.base_link)) + (
2 * rospy.Duration.from_sec(1.0 / self.RATE))
self.tfb.sendTransform(self.saved_translation,
quaternion_from_euler(0, 0, self.saved_yaw),
time, self.odom, self.launch)
r.sleep()
class OptiTrackClient():
def __init__(self, addr, port, obj_names, world, dt, rate=120):
"""
Class tracking optitrack markers through VRPN and publishing their TF frames as well as the transformation
from the robot's world frame to the optitrack frame
:param addr: IP of the VRPN server
:param port: Port of the VRPN server
:param obj_names: Name of the tracked rigid bodies
:param world: Name of the robot's world frame (base_link, map, base, ...)
:param dt: Delta T in seconds whilst a marker is still considered tracked
:param rate: Rate in Hertz of the publishing loop
"""
self.rate = rospy.Rate(rate)
self.obj_names = obj_names
self.world = world
self.dt = rospy.Duration(dt)
self.tfb = TransformBroadcaster()
self.trackers = []
for obj in obj_names:
t = vrpn.receiver.Tracker('{}@{}:{}'.format(obj, addr, port))
t.register_change_handler(obj, self.handler, 'position')
self.trackers.append(t)
self.tracked_objects = {}
@property
def recent_tracked_objects(self):
""" Only returns the objects that have been tracked less than 20ms ago. """
f = lambda name: (rospy.Time.now() - self.tracked_objects[name].
timestamp)
return dict([(k, v) for k, v in self.tracked_objects.iteritems()
if f(k) < self.dt])
def handler(self, obj, data):
self.tracked_objects[obj] = TrackedObject(data['position'],
data['quaternion'],
rospy.Time.now())
def run(self):
while not rospy.is_shutdown():
for t in self.trackers:
t.mainloop()
# Publish the calibration matrix
calib_matrix = rospy.get_param("/optitrack/calibration_matrix")
self.tfb.sendTransform(calib_matrix[0], calib_matrix[1],
rospy.Time.now(), "optitrack_frame",
self.world)
# Publish all other markers as frames
for k, v in self.recent_tracked_objects.iteritems():
self.tfb.sendTransform(v.position, v.quaternion, v.timestamp,
k, "optitrack_frame")
self.rate.sleep()
class Base_pose():
def __init__(self):
rospy.init_node('Base_position', anonymous=True)
self.base_pub = rospy.Publisher('/odometry/filtered_map', Odometry, queue_size=1)
self.br = TransformBroadcaster()
trans = (0,0,0)
rot = (0,0,0,1)
stamp = rospy.Time.now()
self.br.sendTransform(trans, rot, stamp,"base_link","map")
self.Current_goal = []
self.Movedir = [0,0,0]
self.VPose = Odometry()
self.VPose.header.stamp = rospy.Time.now()
self.VPose.header.frame_id = "map"
self.VPose.pose.pose.position.x = float(0)
self.VPose.pose.pose.position.y = float(0)
self.VPose.pose.pose.position.z = float(0)
self.VPose.pose.pose.orientation.x = float(0)
self.VPose.pose.pose.orientation.y = float(0)
self.VPose.pose.pose.orientation.z = float(0)
self.VPose.pose.pose.orientation.w = float(1)
rate = rospy.Rate(1) # 10hz
while not rospy.is_shutdown():
rospy.Subscriber("/move_base_simple/goal", PoseStamped, self.Find_goal, queue_size=1)
if self.Current_goal == []:
self.Movedir = [0,0,0]
else:
Movex = self.Current_goal.pose.position.x-self.VPose.pose.pose.position.x
Movey = self.Current_goal.pose.position.y-self.VPose.pose.pose.position.y
Movez = self.Current_goal.pose.position.z-self.VPose.pose.pose.position.z
Movemag = math.sqrt((Movex)**2+(Movey)**2)
self.Movedir = [Movex, Movey, 0]
#print(self.Movedir)
#self.Movedir = [1,0,0]
self.VPose.header.stamp = rospy.Time.now()
self.VPose.pose.pose.position.x += self.Movedir[0]
self.VPose.pose.pose.position.y += self.Movedir[1]
self.VPose.pose.pose.position.z += self.Movedir[2]
self.br = TransformBroadcaster()
trans = (self.VPose.pose.pose.position.x, self.VPose.pose.pose.position.y, self.VPose.pose.pose.position.z)
rot = (self.VPose.pose.pose.orientation.x, self.VPose.pose.pose.orientation.y, self.VPose.pose.pose.orientation.z, self.VPose.pose.pose.orientation.w)
self.br.sendTransform(trans, rot, self.VPose.header.stamp,"base_link","map") #Pose.header.stamp,"base","map")
self.base_pub.publish(self.VPose)
rate.sleep()
def Find_goal(self,Pose):
if Pose == []:
pass
#self.Current_goal = []
else:
self.Current_goal = Pose
class publish_tag_tf(object):
def __init__(self):
rospy.init_node('republish_tag_tf')
self.tf = TransformListener()
self.br = TransformBroadcaster()
rospy.Subscriber("/tag_detections", AprilTagDetectionArray, self.tags_callback, queue_size=10)
self.tag_detections = rospy.Publisher('/selective_grasping/tag_detections', Int16MultiArray, queue_size=10)
self.detections_list = np.zeros(8)
def tags_callback(self, msg):
detections = len(msg.detections)
self.detections_list = np.zeros(8)
for idx in range(detections):
detected_id = msg.detections[idx].id[0]
self.detections_list[detected_id] = 1
def publish(self, tag):
try:
self.tf.waitForTransform("camera_link", tag, rospy.Time(0), rospy.Duration(1.0)) # rospy.Time.now()
ptFinal, oriFinal = self.tf.lookupTransform("camera_link", tag, rospy.Time(0))
oriFinal_euler = euler_from_quaternion(oriFinal)
new_ori = [0, -0.244, 0]
new_ori[0] = oriFinal_euler[2]
new_ori = quaternion_from_euler(new_ori[0], new_ori[1], new_ori[2])
self.br.sendTransform((ptFinal[2],
-ptFinal[0],
-ptFinal[1]),
new_ori,
rospy.Time.now(),
tag + "_corrected",
"camera_link")
except:
pass
def detection_main(self):
tags = ['tag_0', 'tag_1', 'tag_2', 'tag_3',
'tag_4', 'tag_5', 'tag_6', 'tag_7']
tags_to_send = Int16MultiArray()
rate = rospy.Rate(1)
while not rospy.is_shutdown():
print(self.detections_list)
tags_to_send.data = self.detections_list
self.tag_detections.publish(tags_to_send)
for idx, detected_id in enumerate(self.detections_list):
if detected_id:
self.publish(tags[idx])
else:
print("Tag ID: '{}' not found".format(tags[idx]))
rate.sleep()
class dynamic_transform:
def __init__(self):
rospy.init_node("Dynamic_Transform_odom")
self.br = TransformBroadcaster()
rospy.Subscriber("/odometry/filtered_map",Odometry,self.callback,queue_size=1)
def callback(self, Odom):
trans = (Odom.pose.pose.position.x, Odom.pose.pose.position.y, Odom.pose.pose.position.z)
rot = (Odom.pose.pose.orientation.x, Odom.pose.pose.orientation.y, Odom.pose.pose.orientation.z, Odom.pose.pose.orientation.w)
stamp = rospy.Time.now()
#stamp = Odom.header.stamp
self.br.sendTransform(trans, rot, stamp,"base_link","map")
class Odom_Handler(object):
def __init__(self):
rospy.init_node("Odomer")
self.odom_publisher = rospy.Publisher("odom",Odometry,queue_size = 1)
self.odom_broadcast = TransformBroadcaster()
self.can_listener = rospy.Subscriber("raw_odom", Vector3, callback=self.handle)
self.odom_tf = TransformStamped()
self.odom_msg = Odometry()
#last_rec -> en son alinan ilerleme miktari
self.last_rec = 0
#hesaplanan x, y ve theta degerleri
self.x = 0
self.y = 0
self.th = 0
def handle(self,data):
current_time = rospy.Time.now()
dx = (data.x - self.last_rec) * cos(data.z)
dy = (data.x - self.last_rec) * sin(data.z)
#dth = data.z * dt #self.z eger hiz olarak alinacaksa
self.th += data.z
self.x += dx
self.y += dy
odom_quat = tf.transformations.quaternion_from_euler(0,0,self.th)
self.odom_broadcast.sendTransform((self.x, self.y, 0),
odom_quat,
current_time,
"base_link",
"odom")
self.odom_msg.header.stamp =current_time
self.odom_msg.header.frame_id = "odom"
self.odom_msg.pose.pose.position.x = self.x
self.odom_msg.pose.pose.position.y = self.y
self.odom_msg.pose.pose.position.z = 0.
self.odom_msg.pose.pose.orientation = odom_quat
self.odom_msg.child_frame_id = "base_link"
self.odom_msg.twist.twist.linear.x = data.x
self.odom_msg.twist.twist.angular.z = self.th
#publish msg
self.odom_publisher.publish(self.odom_msg)
self.last_rec = data.x
class TurtlesimToOdomTF():
def __init__(self, name='robot_00', x=5.0, y=5.0, theta=0.0):
rospy.init_node('turtlesim_to_odom')
self.name = name
rospy.wait_for_service('/kill')
kill = rospy.ServiceProxy('/kill',Kill)
if name == 'robot_00':
try:
kill('turtle1')
except:
pass
try:
kill(name)
except:
pass
x = float(x)
y = float(y)
theta = float(theta)
rospy.wait_for_service('/spawn')
spawn = rospy.ServiceProxy('/spawn',Spawn)
try:
spawn(x, y, theta, name)
except:
pass
rospy.Subscriber('pose', TurtlePose, self.pose_callback, queue_size=1)
self.odom_pub = rospy.Publisher('odometry', Odometry, queue_size=1)
self.tfb = TransformBroadcaster()
def pose_callback(self, msg):
pos = (msg.x, msg.y, 0.0)
ori = quaternion_about_axis(msg.theta, (0,0,1))
now = rospy.Time.now()
odom = Odometry()
odom.header.stamp = now
odom.header.frame_id = 'world'
odom.child_frame_id = self.name
odom.pose.pose = Pose(Point(*pos),Quaternion(*ori))
self.odom_pub.publish(odom)
self.tfb.sendTransform(pos, ori, now, self.name, 'world')
def run(self):
rospy.spin()
class OptiTrackClient():
def __init__(self, addr, port, obj_names, world, dt, rate=120):
"""
Class tracking optitrack markers through VRPN and publishing their TF frames as well as the transformation
from the robot's world frame to the optitrack frame
:param addr: IP of the VRPN server
:param port: Port of the VRPN server
:param obj_names: Name of the tracked rigid bodies
:param world: Name of the robot's world frame (base_link, map, base, ...)
:param dt: Delta T in seconds whilst a marker is still considered tracked
:param rate: Rate in Hertz of the publishing loop
"""
self.rate = rospy.Rate(rate)
self.obj_names = obj_names
self.world = world
self.dt = rospy.Duration(dt)
self.tfb = TransformBroadcaster()
self.trackers = []
for obj in obj_names:
t = vrpn.receiver.Tracker('{}@{}:{}'.format(obj, addr, port))
t.register_change_handler(obj, self.handler, 'position')
self.trackers.append(t)
self.tracked_objects = {}
@property
def recent_tracked_objects(self):
""" Only returns the objects that have been tracked less than 20ms ago. """
f = lambda name: (rospy.Time.now() - self.tracked_objects[name].timestamp)
return dict([(k, v) for k, v in self.tracked_objects.iteritems() if f(k) < self.dt])
def handler(self, obj, data):
self.tracked_objects[obj] = TrackedObject(data['position'],
data['quaternion'],
rospy.Time.now())
def run(self):
while not rospy.is_shutdown():
for t in self.trackers:
t.mainloop()
# Publish the calibration matrix
calib_matrix = rospy.get_param("/optitrack/calibration_matrix")
self.tfb.sendTransform(calib_matrix[0], calib_matrix[1], rospy.Time.now(), "optitrack_frame", self.world)
# Publish all other markers as frames
for k, v in self.recent_tracked_objects.iteritems():
self.tfb.sendTransform(v.position, v.quaternion, v.timestamp, k, "optitrack_frame")
self.rate.sleep()
class Rebroadcaster(object):
def __init__(self):
self.broadcaster = TransformBroadcaster()
self.ell_param_sub = rospy.Subscriber('ellipsoid_params', EllipsoidParams, self.ell_cb)
self.transform = None
def ell_cb(self, ell_msg):
print "Got transform"
self.transform = copy.copy(ell_msg.e_frame)
def send_transform(self):
print "spinning", self.transform
if self.transform is not None:
print "Sending frame"
self.broadcaster.sendTransform(self.transform)
class Sync_base():
def __init__(self):
rospy.init_node('Sync_base', anonymous=True)
self.br = TransformBroadcaster()
trans = (0,0,0)
rot = (0,0,0,1)
stamp = rospy.Time.now()
self.br.sendTransform(trans, rot, stamp,"base","map")
rospy.Subscriber("/odometry/filtered_map", Odometry, self.base_pose, queue_size=1)
def base_pose(self,Pose):
self.br = TransformBroadcaster()
trans = (Pose.pose.pose.position.x, Pose.pose.pose.position.y, Pose.pose.pose.position.z)
rot = (Pose.pose.pose.orientation.x, Pose.pose.pose.orientation.y, Pose.pose.pose.orientation.z, Pose.pose.pose.orientation.w)
self.br.sendTransform(trans, rot, rospy.Time.now(),"base","map") #Pose.header.stamp,"base","map")
class Rebroadcaster(object):
def __init__(self):
self.broadcaster = TransformBroadcaster()
self.ell_param_sub = rospy.Subscriber('ellipsoid_params',
EllipsoidParams, self.ell_cb)
self.transform = None
def ell_cb(self, ell_msg):
print "Got transform"
self.transform = copy.copy(ell_msg.e_frame)
def send_transform(self):
print "spinning", self.transform
if self.transform is not None:
print "Sending frame"
self.broadcaster.sendTransform(self.transform)
def callback(ref_sub, odom_sub, odom_pub):
Refer = Odometry()
Refer = ref_sub
# Odom = Odometry()
# Odom = odom_sub
odom_broadcaster = TransformBroadcaster()
current_time = rospy.Time.now
# last_time = rospy.Time.now
rospy.loginfo('AAAAAAAAAAAA')
current_time = rospy.Time.now
# dt = rospy.Time.to_sec(current_time-last_time)
# odom_trans = TransformStamped()
# odom_trans.header.stamp = current_time
# odom_trans.header.frame_id = "odom"
# odom_trans.child_frame_id = "base_footprint"
# odom_trans.transform.translation.x = Refer.pose.pose.position.x
# odom_trans.transform.translation.y = Refer.pose.pose.position.y
# odom_trans.transform.translation.z = 0.0
# odom_trans.transform.rotation = Refer.pose.pose.orientation
odom_quat = Refer.pose.pose.orientation
odom_broadcaster.sendTransform(
(Refer.pose.pose.position.x, Refer.pose.pose.position.y, 0.),
(Refer.pose.pose.orientation.x,Refer.pose.pose.orientation.y,Refer.pose.pose.orientation.z,Refer.pose.pose.orientation.w),
current_time,
"base_footprint",
"odom"
)
# odom_broadcaster.sendTransform(odom_trans)
#next, we'll publish the odometry message over ROS
odom = Odometry()
odom.header.stamp = rospy.Time.now()
odom.header.frame_id = "odom"
print('AAAAAAAAAAAAAAAAAAAAAAAAAAAAAA')
#set the position
odom.pose.pose.position.x = Refer.pose.pose.position.x
odom.pose.pose.position.y = Refer.pose.pose.position.y
odom.pose.pose.position.z = 0.0
odom.pose.pose.orientation = Refer.pose.pose.orientation
#set the velocity
odom.child_frame_id = "base_footprint"
odom.twist.twist.linear.x = Refer.twist.twist.linear.x
odom.twist.twist.linear.y = Refer.twist.twist.linear.y
odom.twist.twist.angular.z = Refer.twist.twist.angular.z
#publish the message
odom_pub.publish(odom)
def callback(data):
rospy.loginfo(rospy.get_caller_id() + "%s", data.data)
x, th = data.data.split(',')
b = TransformBroadcaster()
translation = (0.0, 0.0, 0.0)
rotation = (0.0, 0.0, 0.0, 0.0)
#rate = rospy.Rate(5) # 5hz
x = float(x)
th = float(th)
translation = (x, 0.0, 0.0)
rotation = (0.0, 0.0, 0.0, th)
b.sendTransform(translation, rotation, Time.now(), 'Raiden', '/world')
def get_model_tf_cb(msg):
"""Read C5 model state and publish to tf tree as transform between /world --> /c5_link
Args:
msg (gazebo/ModelStates): model states in world frame
"""
# get the index of the model name
i = msg.name.index(MODEL_NAME)
# construct tf data
translation = (msg.pose[i].position.x, msg.pose[i].position.y,
msg.pose[i].position.z)
rotation = (msg.pose[i].orientation.x, msg.pose[i].orientation.y,
msg.pose[i].orientation.z, msg.pose[i].orientation.w)
# publish tf
b = TransformBroadcaster()
b.sendTransform(translation, rotation, Time.now(), '/c5_link', '/world')
class CorrectOdomNode:
def __init__(self):
# listen to tf messages to get transfrom from odom->base_link
self.tfl = TransformListener()
# broadcast world->odom transform
self.tfb = TransformBroadcaster()
# @todo: get frame ids as parameters
self.base_frame_id = 'base_link'
self.odom_frame_id = 'odom'
# note: world frame id provided by the PoseStamped header
self.sub = rospy.Subscriber("pose_stamped", PoseStamped, self.pose_callback)
def pose_callback(self, msg):
# base to odom (b2o) transform
try:
(b2o_trans, b2o_rot) = self.tfl.lookupTransform(self.base_frame_id, self.odom_frame_id, rospy.Time(0))
except (LookupException, ConnectivityException, ExtrapolationException):
rospy.logwarn_throttle(10, "tf exception in pose_callback()... ignoring")
return
b2o = transformations.concatenate_matrices(transformations.translation_matrix(b2o_trans),
transformations.quaternion_matrix(b2o_rot))
# world to base (w2b) transform
w2b_trans = transformations.translation_matrix((msg.pose.position.x,
msg.pose.position.y,
msg.pose.position.z))
w2b_rot = transformations.quaternion_matrix((msg.pose.orientation.x,
msg.pose.orientation.y,
msg.pose.orientation.z,
msg.pose.orientation.w))
w2b = transformations.concatenate_matrices(w2b_trans, w2b_rot)
# world to base * base to odom = world to odom (w2o)
w2o = transformations.concatenate_matrices(w2b, b2o)
# broadcast world to odom
w2o_trans = transformations.translation_from_matrix(w2o)
w2o_rot = transformations.quaternion_from_matrix(w2o)
self.tfb.sendTransform(w2o_trans, w2o_rot, msg.header.stamp, self.odom_frame_id, msg.header.frame_id)
def ggcnn_command_callback(self, msg):
"""
GGCNN Command Subscriber Callback
"""
self.tf.waitForTransform("base_link", "object_detected", rospy.Time(0),
rospy.Duration(4.0)) # rospy.Time.now()
object_pose, object_ori = self.tf.lookupTransform(
"base_link", "object_detected", rospy.Time(0))
self.d = list(msg.data)
object_pose[0] += self.GGCNN_offset_x
object_pose[1] += self.GGCNN_offset_y
object_pose[2] += self.GGCNN_offset_z
self.posCB = object_pose
self.ori = self.d[3]
br = TransformBroadcaster()
br.sendTransform((object_pose[0], object_pose[1], object_pose[2]),
quaternion_from_euler(0.0, 0.0, self.ori),
rospy.Time.now(), "object_link", "base_link")
def main():
rospy.init_node('my_broadcaster')
b = TransformBroadcaster()
translation = (0.0, 0.0, 0.0)
rotation = (0.0, 0.0, 0.0, 1.0)
rate = rospy.Rate(5) # 5hz
x, y = 0.0, 0.0
while not rospy.is_shutdown():
if x >= 2:
x, y = 0.0, 0.0
x += 0.1
y += 0.1
translation = (x, y, 0.0)
b.sendTransform(translation, rotation, Time.now(), '/odom', '/world')
b.sendTransform(translation, rotation, Time.now(), '/laser',
'/chassis')
b.sendTransform(translation, rotation, Time.now(), '/camera1',
'/chassis')
rate.sleep()
def handle_car_odom(msg):
br = TransformBroadcaster()
# t = TransformStamped()
# t.header.stamp = rospy.Time.now()
# t.header.frame_id = "world"
# t.child_frame_id = tfPrefix + "/" + msg.child_frame_id
# t.transform.translation.x = msg.pose.pose.position.x
# t.transform.translation.y = msg.pose.pose.position.y
# t.transform.translation.z = msg.pose.pose.position.z
# t.transform.rotation.x = msg.pose.pose.orientation.x
# t.transform.rotation.y = msg.pose.pose.orientation.y
# t.transform.rotation.z = msg.pose.pose.orientation.z
# t.transform.rotation.w = msg.pose.pose.orientation.w
br.sendTransform((msg.pose.pose.position.x, msg.pose.pose.position.y, msg.pose.pose.position.z),
(msg.pose.pose.orientation.x, msg.pose.pose.orientation.y, msg.pose.pose.orientation.z, msg.pose.pose.orientation.w),
rospy.Time.now(),
tfPrefix + "/" + msg.child_frame_id,
"world")
def move_robot_model_rotate():
translation = (0.0, 0.0, 0.0)
rotation = (0.0, 3.0, 0.0, 1.0)
b = TransformBroadcaster()
b.sendTransform(translation, rotation, Time.now(), 'base_link', '/map')
b.sendTransform(translation, rotation, Time.now(), 'camera_link', '/map')
b.sendTransform(translation, rotation, Time.now(),
'camera_link_normalized', '/map')
class Left_Utility_Frame():
frame = 'base_footprint'
px = py = pz = 0
qx = qy = qz = 0
qw = 1
def __init__(self):
rospy.init_node('left_utilitiy_frame_source')
self.updater = rospy.Service('l_utility_frame_update', FrameUpdate,
self.update_frame)
self.tfb = TransformBroadcaster()
def update_frame(self, req):
ps = req.pose
if not (math.isnan(ps.pose.orientation.x) or math.isnan(
ps.pose.orientation.y) or math.isnan(ps.pose.orientation.z)
or math.isnan(ps.pose.orientation.w)):
self.frame = ps.header.frame_id
self.px = ps.pose.position.x
self.py = ps.pose.position.y
self.pz = ps.pose.position.z
self.qx = ps.pose.orientation.x
self.qy = ps.pose.orientation.y
self.qz = ps.pose.orientation.z
self.qw = ps.pose.orientation.w
else:
rospy.logerr("NAN's sent to l_utility_frame_source")
self.tfb.sendTransform(
(self.px, self.py, self.pz), (self.qx, self.qy, self.qz, self.qw),
rospy.Time.now(), "lh_utility_frame", self.frame)
rsp = Bool()
rsp.data = True
return rsp
class EllipsoidSpace(object):
def __init__(self, E=1, is_oblate=False):
self.A = 1
self.E = E
#self.B = np.sqrt(1. - E**2)
self.a = self.A * self.E
self.is_oblate = is_oblate
self.center = None
self.frame_broadcaster = TransformBroadcaster()
self.center_tf_timer = None
def load_ell_params(self, ell_pose, E, is_oblate=False, height=1):
rospy.loginfo("Loading Ellipsoid Parameters")
self.set_center(ell_pose)
self.E = E
self.a = self.A * self.E
self.is_oblate = is_oblate
self.height = height
def set_center(self, pose_stamped):
rospy.loginfo("[ellipsoid_space] Setting center to:\r\n %s" %pose_stamped)
if self.center_tf_timer is not None:
self.center_tf_timer.shutdown()
self.center = pose_stamped
def broadcast_ell_center(event):
tr = (pose_stamped.pose.position.x,
pose_stamped.pose.position.y,
pose_stamped.pose.position.z)
quat = (pose_stamped.pose.orientation.x,
pose_stamped.pose.orientation.y,
pose_stamped.pose.orientation.z,
pose_stamped.pose.orientation.w)
self.frame_broadcaster.sendTransform(tr, quat, rospy.Time.now(),
'/ellipse_frame',
self.center.header.frame_id)
self.center_tf_timer = rospy.Timer(rospy.Duration(0.01), broadcast_ell_center)
def set_bounds(self, lat_bounds=None, lon_bounds=None, height_bounds=None):
assert lon_bounds[1] >= 0
self._lat_bounds = lat_bounds
self._lon_bounds = lon_bounds
self._height_bounds = height_bounds
def enforce_bounds(self, ell_pos):
lat = np.clip(ell_pos[0], self._lat_bounds[0], self._lat_bounds[1])
if self._lon_bounds[0] >= 0:
lon = np.clip(ell_pos[1], self._lon_bounds[0], self._lon_bounds[1])
else:
ell_lon_1 = np.mod(ell_pos[1], 2 * np.pi)
min_lon = np.mod(self._lon_bounds[0], 2 * np.pi)
if (ell_lon_1 >= min_lon) or (ell_lon_1 <= self._lon_bounds[1]):
lon = ell_pos[1]
else:
if min_lon - ell_lon_1 < ell_lon_1 - self._lon_bounds[1]:
lon = min_lon
else:
lon = self._lon_bounds[1]
height = np.clip(ell_pos[2], self._height_bounds[0], self._height_bounds[1])
return np.array([lat, lon, height])
def ellipsoidal_to_cart(self, lat, lon, height):
#assert height > 0 and lat >= 0 and lat <= np.pi and lon >= 0 and lon < 2 * np.pi
if not self.is_oblate:
x = self.a * np.sinh(height) * np.sin(lat) * np.cos(lon)
y = self.a * np.sinh(height) * np.sin(lat) * np.sin(lon)
z = self.a * np.cosh(height) * np.cos(lat)
else:
x = self.a * np.cosh(height) * np.cos(lat-np.pi/2) * np.cos(lon)
y = self.a * np.cosh(height) * np.cos(lat-np.pi/2) * np.sin(lon)
z = self.a * np.sinh(height) * np.sin(lat-np.pi/2)
pos_local = np.mat([x, y, z]).T
return pos_local
def partial_height(self, lat, lon, height):
#assert height > 0 and lat >= 0 and lat <= np.pi and lon >= 0 and lon < 2 * np.pi
if not self.is_oblate:
x = self.a * np.cosh(height) * np.sin(lat) * np.cos(lon)
y = self.a * np.cosh(height) * np.sin(lat) * np.sin(lon)
z = self.a * np.sinh(height) * np.cos(lat)
else:
x = self.a * np.sinh(height) * np.sin(lat-np.pi/2) * np.cos(lon)
y = self.a * np.sinh(height) * np.sin(lat-np.pi/2) * np.sin(lon)
z = self.a * np.cosh(height) * np.cos(lat-np.pi/2)
return np.mat([x, y, z]).T
#def partial_v(self, lat, lon, height):
# #assert height > 0 and lat >= 0 and lat <= np.pi and lon >= 0 and lon < 2 * np.pi
# x = self.a * np.sinh(height) * np.cos(lat) * np.cos(lon)
# y = self.a * np.sinh(height) * np.cos(lat) * np.sin(lon)
# z = self.a * np.cosh(height) * -np.sin(lat)
# return np.mat([x, y, z]).T
#def partial_p(self, lat, lon, height):
# #assert height > 0 and lat >= 0 and lat <= np.pi and lon >= 0 and lon < 2 * np.pi
# x = self.a * np.sinh(height) * np.sin(lat) * -np.sin(lon)
# y = self.a * np.sinh(height) * np.sin(lat) * np.cos(lon)
# z = 0.
# return np.mat([x, y, z]).T
def ellipsoidal_to_pose(self, pose):
ell_pos, ell_quat = PoseConv.to_pos_quat(pose)
if not self.is_oblate:
return self._ellipsoidal_to_pose_prolate(ell_pos, ell_quat)
else:
return self._ellipsoidal_to_pose_oblate(ell_pos, ell_quat)
def _ellipsoidal_to_pose_prolate(self, ell_pos, ell_quat):
pos = self.ellipsoidal_to_cart(ell_pos[0], ell_pos[1], ell_pos[2])
df_du = self.partial_height(ell_pos[0], ell_pos[1], ell_pos[2])
nx, ny, nz = df_du.T.A[0] / np.linalg.norm(df_du)
j = np.sqrt(1./(1.+ny*ny/(nz*nz)))
k = -ny*j/nz
norm_rot = np.mat([[-nx, ny*k - nz*j, 0],
[-ny, -nx*k, j],
[-nz, nx*j, k]])
_, norm_quat = PoseConv.to_pos_quat(np.mat([0, 0, 0]).T, norm_rot)
rot_angle = np.arctan(-norm_rot[2,1] / norm_rot[2,2])
#print norm_rot
quat_ortho_rot = trans.quaternion_from_euler(rot_angle + np.pi, 0.0, 0.0)
norm_quat_ortho = trans.quaternion_multiply(norm_quat, quat_ortho_rot)
norm_rot_ortho = np.mat(trans.quaternion_matrix(norm_quat_ortho)[:3,:3])
if norm_rot_ortho[2, 2] > 0:
flip_axis_ang = 0
else:
flip_axis_ang = np.pi
quat_flip = trans.quaternion_from_euler(flip_axis_ang, 0.0, 0.0)
norm_quat_ortho_flipped = trans.quaternion_multiply(norm_quat_ortho, quat_flip)
ell_frame_quat = trans.quaternion_multiply(norm_quat_ortho_flipped, ell_quat)
pose = PoseConv.to_pos_quat(pos, ell_frame_quat)
#print ("ellipsoidal_to_pose: latlonheight: %f, %f, %f" %
# (lat, lon, height) +
# str(PoseConv.to_homo_mat(pose)))
return pose
def _ellipsoidal_to_pose_oblate(self, ell_pos, ell_quat):
pos = self.ellipsoidal_to_cart(ell_pos[0], ell_pos[1], ell_pos[2])
df_du = self.partial_height(-ell_pos[0], ell_pos[1], ell_pos[2])
nx, ny, nz = df_du.T.A[0] / np.linalg.norm(df_du)
j = np.sqrt(1./(1.+ny*ny/(nz*nz)))
k = -ny*j/nz
norm_rot = np.mat([[-nx, ny*k - nz*j, 0],
[-ny, -nx*k, j],
[-nz, nx*j, k]])
_, norm_quat = PoseConv.to_pos_quat(np.mat([0, 0, 0]).T, norm_rot)
rot_angle = np.arctan(-norm_rot[2,1] / norm_rot[2,2])
#print norm_rot
quat_ortho_rot = trans.quaternion_from_euler(rot_angle, 0.0, 0.0)
norm_quat_ortho = trans.quaternion_multiply(norm_quat, quat_ortho_rot)
quat_ortho_rot2 = trans.quaternion_from_euler(0.0, np.pi/2, 0.0)
norm_quat_ortho = trans.quaternion_multiply(norm_quat_ortho, quat_ortho_rot2)
if lon >= np.pi:
quat_flip = trans.quaternion_from_euler(0.0, 0.0, np.pi)
norm_quat_ortho = trans.quaternion_multiply(norm_quat_ortho, quat_flip)
ell_frame_quat = trans.quaternion_multiply(norm_quat_ortho, ell_quat)
pose = PoseConv.to_pos_quat(pos, norm_quat_ortho)
#print ("ellipsoidal_to_pose: latlonheight: %f, %f, %f" %
# (lat, lon, height) +
# str(PoseConv.to_homo_mat(pose)))
return pose
def normal_to_ellipse(self, lat, lon, height):
print "Finding ell_to_pose"
if not self.is_oblate:
return self._normal_to_ellipse_prolate(lat, lon, height)
else:
return self._normal_to_ellipse_oblate(lat, lon, height)
def _normal_to_ellipse_prolate(self, lat, lon, height):
pos = self.ellipsoidal_to_cart(lat, lon, height)
df_du = self.partial_height(lat, lon, height)
nx, ny, nz = df_du.T.A[0] / np.linalg.norm(df_du)
j = np.sqrt(1./(1.+ny*ny/(nz*nz)))
k = -ny*j/nz
norm_rot = np.mat([[-nx, ny*k - nz*j, 0],
[-ny, -nx*k, j],
[-nz, nx*j, k]])
_, norm_quat = PoseConv.to_pos_quat(np.mat([0, 0, 0]).T, norm_rot)
rot_angle = np.arctan(-norm_rot[2,1] / norm_rot[2,2])
#print norm_rot
quat_ortho_rot = trans.quaternion_from_euler(rot_angle + np.pi, 0.0, 0.0)
norm_quat_ortho = trans.quaternion_multiply(norm_quat, quat_ortho_rot)
norm_rot_ortho = np.mat(trans.quaternion_matrix(norm_quat_ortho)[:3,:3])
if norm_rot_ortho[2, 2] > 0:
flip_axis_ang = 0
else:
flip_axis_ang = np.pi
quat_flip = trans.quaternion_from_euler(flip_axis_ang, 0.0, 0.0)
norm_quat_ortho_flipped = trans.quaternion_multiply(norm_quat_ortho, quat_flip)
pose = PoseConv.to_pos_quat(pos, norm_quat_ortho_flipped)
#print ("ellipsoidal_to_pose: latlonheight: %f, %f, %f" %
# (lat, lon, height) +
# str(PoseConv.to_homo_mat(pose)))
return pose
def _normal_to_ellipse_oblate(self, lat, lon, height):
pos = self.ellipsoidal_to_cart(lat, lon, height)
df_du = self.partial_height(-lat, lon, height)
nx, ny, nz = df_du.T.A[0] / np.linalg.norm(df_du)
j = np.sqrt(1./(1.+ny*ny/(nz*nz)))
k = -ny*j/nz
norm_rot = np.mat([[-nx, ny*k - nz*j, 0],
[-ny, -nx*k, j],
[-nz, nx*j, k]])
_, norm_quat = PoseConv.to_pos_quat(np.mat([0, 0, 0]).T, norm_rot)
rot_angle = np.arctan(-norm_rot[2,1] / norm_rot[2,2])
#print norm_rot
quat_ortho_rot = trans.quaternion_from_euler(rot_angle, 0.0, 0.0)
norm_quat_ortho = trans.quaternion_multiply(norm_quat, quat_ortho_rot)
quat_ortho_rot2 = trans.quaternion_from_euler(0.0, np.pi/2, 0.0)
norm_quat_ortho = trans.quaternion_multiply(norm_quat_ortho, quat_ortho_rot2)
if lon >= np.pi:
quat_flip = trans.quaternion_from_euler(0.0, 0.0, np.pi)
norm_quat_ortho = trans.quaternion_multiply(norm_quat_ortho, quat_flip)
pose = PoseConv.to_pos_quat(pos, norm_quat_ortho)
#print ("ellipsoidal_to_pose: latlonheight: %f, %f, %f" %
# (lat, lon, height) +
# str(PoseConv.to_homo_mat(pose)))
return pose
def pose_to_ellipsoidal(self, pose):
pose_pos, pose_rot = PoseConv.to_pos_rot(pose)
lat, lon, height = self.pos_to_ellipsoidal(pose_pos[0,0], pose_pos[1,0], pose_pos[2,0])
_, ell_rot = PoseConv.to_pos_rot(self.normal_to_ellipse(lat, lon, height))
_, quat_rot = PoseConv.to_pos_quat(np.mat([0]*3).T, ell_rot.T * pose_rot)
return [lat, lon, height], quat_rot
def pos_to_ellipsoidal(self, x, y, z):
if not self.is_oblate:
return self._pos_to_ellipsoidal_prolate(x, y, z)
else:
return self._pos_to_ellipsoidal_oblate(x, y, z)
def _pos_to_ellipsoidal_prolate(self, x, y, z):
lon = np.arctan2(y, x)
if lon < 0.:
lon += 2 * np.pi
p = np.sqrt(x**2 + y**2)
a = self.a
inner = np.sqrt((np.sqrt((z**2 - a**2 + p**2)**2 + (2. * a * p)**2) / a**2 -
(z / a)**2 - (p / a)**2 + 1) / 2.)
assert inner < 1.0001
if inner > 0.9999:
lat = np.pi/2.
else:
lat = np.arcsin(inner)
if z < 0.:
lat = np.pi - np.fabs(lat)
if np.fabs(np.sin(lat)) > 0.05:
if np.fabs(np.cos(lon)) > 0.05:
use_case = 'x'
sinh_height = x / (a * np.sin(lat) * np.cos(lon))
height = np.log(sinh_height + np.sqrt(sinh_height**2 + 1))
else:
use_case = 'y'
sinh_height = y / (a * np.sin(lat) * np.sin(lon))
height = np.log(sinh_height + np.sqrt(sinh_height**2 + 1))
else:
use_case = 'z'
cosh_height = z / (a * np.cos(lat))
assert np.fabs(cosh_height) >= 1, ("cosh_height %f, a %f, x %f, y %f, z %f, lat %f, lon %f" %
(cosh_height, a, x, y, z, lat, lon))
height = np.log(cosh_height + np.sqrt(cosh_height**2 - 1))
print ("%s pos_to_ellipsoidal: xyz: %f, %f, %f; latlonheight: %f, %f, %f" %
(use_case, x, y, z, lat, lon, height))
assert not np.any(np.isnan([lat, lon, height])), ("cosh_height %f, a %f, x %f, y %f, z %f, lat %f, lon %f" %
(cosh_height, a, x, y, z, lat, lon))
return lat, lon, height
def _pos_to_ellipsoidal_oblate(self, x, y, z):
lon = np.arctan2(y, x)
if lon < 0.:
lon += 2 * np.pi
p = np.sqrt(x**2 + y**2)
a = self.a
d_1 = np.sqrt((p + a)**2 + z**2)
d_2 = np.sqrt((p - a)**2 + z**2)
cosh_height = (d_1 + d_2) / (2 * a)
assert np.fabs(cosh_height) >= 1, ("cosh_height %f, a %f, x %f, y %f, z %f, lat %f, lon %f" %
(cosh_height, a, x, y, z, lat, lon))
height = np.log(cosh_height + np.sqrt(cosh_height**2 - 1))
cos_lat = (d_1 - d_2) / (2 * a)
lat = np.arccos(cos_lat)
if z < 0.:
lat *= -1
# we're going to convert the latitude coord so it is always positive:
lat += np.pi / 2.
return lat, lon, height
def create_ellipsoidal_path(self, start_ell_pos, start_ell_quat,
end_ell_pos, end_ell_quat,
velocity=0.001, min_jerk=True):
print "Start rot (%s):\r\n%s" %(type(start_ell_quat),start_ell_quat)
print "End rot (%s):\r\n%s" %(type(end_ell_quat),end_ell_quat)
_, start_ell_rot = PoseConv.to_pos_rot((start_ell_pos,start_ell_quat))
_, end_ell_rot = PoseConv.to_pos_rot((end_ell_pos,end_ell_quat))
rpy = trans.euler_from_matrix(start_ell_rot.T * end_ell_rot) # get roll, pitch, yaw of angle diff
end_ell_pos[1] = np.mod(end_ell_pos[1], 2 * np.pi) # wrap longitude value
ell_init = np.mat(start_ell_pos).T
ell_final = np.mat(end_ell_pos).T
# find the closest longitude angle to interpolate to
if np.fabs(2 * np.pi + ell_final[1,0] - ell_init[1,0]) < np.pi:
ell_final[1,0] += 2 * np.pi
elif np.fabs(-2 * np.pi + ell_final[1,0] - ell_init[1,0]) < np.pi:
ell_final[1,0] -= 2 * np.pi
if np.any(np.isnan(ell_init)) or np.any(np.isnan(ell_final)):
rospy.logerr("[ellipsoid_space] Nan values in ellipsoid. " +
"ell_init: %f, %f, %f; " % (ell_init[0,0], ell_init[1,0], ell_init[2,0]) +
"ell_final: %f, %f, %f; " % (ell_final[0,0], ell_final[1,0], ell_final[2,0]))
return None
num_samps = np.max([2, int(np.linalg.norm(ell_final - ell_init) / velocity),
int(np.linalg.norm(rpy) / velocity)])
if min_jerk:
t_vals = min_jerk_traj(num_samps)
else:
t_vals = np.linspace(0,1,num_samps)
# smoothly interpolate from init to final
ell_lat_traj = np.interp(t_vals, (0,1),(start_ell_pos[0], end_ell_pos[0]))
ell_lon_traj = np.interp(t_vals, (0,1),(start_ell_pos[1], end_ell_pos[1]))
ell_height_traj = np.interp(t_vals, (0,1),(start_ell_pos[2], end_ell_pos[2]))
ell_pos_traj = np.vstack((ell_lat_traj, ell_lon_traj, ell_height_traj))
ell_quat_traj = [trans.quaternion_slerp(start_ell_quat, end_ell_quat, t) for t in t_vals]
return [(ell_pos_traj[:,i], ell_quat_traj[i]) for i in xrange(num_samps)]
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 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 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)
class GazeEstimatorROS(GazeEstimatorBase):
def __init__(self, device_id_gaze, model_files):
super(GazeEstimatorROS, self).__init__(device_id_gaze, model_files)
self.bridge = CvBridge()
self.subjects_bridge = SubjectListBridge()
self.tf_broadcaster = TransformBroadcaster()
self.tf_listener = TransformListener()
self.tf_prefix = rospy.get_param("~tf_prefix", "gaze")
self.headpose_frame = self.tf_prefix + "/head_pose_estimated"
self.ros_tf_frame = rospy.get_param("~ros_tf_frame",
"/kinect2_ros_frame")
self.image_subscriber = rospy.Subscriber("/subjects/images",
MSG_SubjectImagesList,
self.image_callback,
queue_size=3,
buff_size=2**24)
self.subjects_gaze_img = rospy.Publisher("/subjects/gazeimages",
Image,
queue_size=3)
self.visualise_eyepose = rospy.get_param("~visualise_eyepose",
default=True)
self._last_time = rospy.Time().now()
def publish_image(self, image, image_publisher, timestamp):
"""This image publishes the `image` to the `image_publisher` with the given `timestamp`."""
image_ros = self.bridge.cv2_to_imgmsg(image, "rgb8")
image_ros.header.stamp = timestamp
image_publisher.publish(image_ros)
def image_callback(self, subject_image_list):
"""This method is called whenever new input arrives. The input is first converted in a format suitable
for the gaze estimation network (see :meth:`input_from_image`), then the gaze is estimated (see
:meth:`estimate_gaze`. The estimated gaze is overlaid on the input image (see :meth:`visualize_eye_result`),
and this image is published along with the estimated gaze vector (see :meth:`publish_image` and
:func:`publish_gaze`)"""
timestamp = subject_image_list.header.stamp
subjects_dict = self.subjects_bridge.msg_to_images(subject_image_list)
input_r_list = []
input_l_list = []
input_head_list = []
valid_subject_list = []
for subject_id, s in subjects_dict.items():
try:
(trans_head, rot_head) = self.tf_listener.lookupTransform(
self.ros_tf_frame, self.headpose_frame + str(subject_id),
timestamp)
euler_angles_head = list(
tf.transformations.euler_from_quaternion(rot_head))
euler_angles_head = gaze_tools.limit_yaw(euler_angles_head)
phi_head, theta_head = gaze_tools.get_phi_theta_from_euler(
euler_angles_head)
input_head_list.append([theta_head, phi_head])
input_r_list.append(self.input_from_image(s.right))
input_l_list.append(self.input_from_image(s.left))
valid_subject_list.append(subject_id)
except (tf.LookupException, tf.ConnectivityException,
tf.ExtrapolationException, tf.Exception):
pass
if len(valid_subject_list) == 0:
return
gaze_est = self.estimate_gaze_twoeyes(
inference_input_left_list=input_l_list,
inference_input_right_list=input_r_list,
inference_headpose_list=input_head_list)
subjects_gaze_img_list = []
for subject_id, gaze in zip(valid_subject_list, gaze_est.tolist()):
self.publish_gaze(gaze, timestamp, subject_id)
if self.visualise_eyepose:
s = subjects_dict[subject_id]
r_gaze_img = self.visualize_eye_result(s.right, gaze)
l_gaze_img = self.visualize_eye_result(s.left, gaze)
s_gaze_img = np.concatenate((r_gaze_img, l_gaze_img), axis=1)
subjects_gaze_img_list.append(s_gaze_img)
if len(subjects_gaze_img_list) > 0:
gaze_img_msg = self.bridge.cv2_to_imgmsg(
np.hstack(subjects_gaze_img_list).astype(np.uint8), "bgr8")
gaze_img_msg.header.stamp = timestamp
self.subjects_gaze_img.publish(gaze_img_msg)
_now = rospy.Time().now()
_freq = 1.0 / (_now - self._last_time).to_sec()
self._last_time = _now
tqdm.write(
'\033[2K\033[1;32mTime now: {:.2f} message color: {:.2f} diff: {:.2f}s for {} subjects {:.0f}Hz\033[0m'
.format((_now.to_sec()), timestamp.to_sec(),
_now.to_sec() - timestamp.to_sec(),
len(valid_subject_list), _freq),
end="\r")
def publish_gaze(self, est_gaze, msg_stamp, subject_id):
"""Publish the gaze vector as a PointStamped."""
theta_gaze = est_gaze[0]
phi_gaze = est_gaze[1]
euler_angle_gaze = gaze_tools.get_euler_from_phi_theta(
phi_gaze, theta_gaze)
quaternion_gaze = tf.transformations.quaternion_from_euler(
*euler_angle_gaze)
self.tf_broadcaster.sendTransform(
(0, 0,
0.05), # publish it 5cm above the head pose's origin (nose tip)
quaternion_gaze,
msg_stamp,
self.tf_prefix + "/world_gaze" + str(subject_id),
self.headpose_frame + str(subject_id))
class World:
'''Object recognition and localization related stuff'''
tf_listener = None
objects = []
def __init__(self):
if World.tf_listener == None:
World.tf_listener = TransformListener()
self._lock = threading.Lock()
self.surface = None
self._tf_broadcaster = TransformBroadcaster()
self._im_server = InteractiveMarkerServer('world_objects')
bb_service_name = 'find_cluster_bounding_box'
rospy.wait_for_service(bb_service_name)
self._bb_service = rospy.ServiceProxy(bb_service_name,
FindClusterBoundingBox)
rospy.Subscriber('interactive_object_recognition_result',
GraspableObjectList, self.receive_object_info)
self._object_action_client = actionlib.SimpleActionClient(
'object_detection_user_command', UserCommandAction)
self._object_action_client.wait_for_server()
rospy.loginfo('Interactive object detection action ' +
'server has responded.')
self.clear_all_objects()
# The following is to get the table information
rospy.Subscriber('tabletop_segmentation_markers',
Marker, self.receive_table_marker)
rospy.Subscriber("ar_pose_marker",
AlvarMarkers, self.receive_ar_markers)
self.marker_dims = Vector3(0.04, 0.04, 0.01)
def receive_ar_markers(self, data):
'''Callback function to receive marker info'''
self._lock.acquire()
if len(data.markers) > 0:
rospy.loginfo('Received non-empty marker list.')
for i in range(len(data.markers)):
marker = data.markers[i]
self._add_new_object(marker.pose.pose, self.marker_dims, marker.id)
self._lock.release()
def _reset_objects(self):
'''Function that removes all objects'''
self._lock.acquire()
for i in range(len(World.objects)):
self._im_server.erase(World.objects[i].int_marker.name)
self._im_server.applyChanges()
if self.surface != None:
self._remove_surface()
self._im_server.clear()
self._im_server.applyChanges()
World.objects = []
self._lock.release()
def get_tool_id(self):
if (len(self.objects) == 0):
rospy.warning('There are no fiducials, cannot get tool ID.')
return None
elif (len(self.objects) > 1):
rospy.warning('There are more than one fiducials, returning the first as tool ID.')
return World.objects[0].marker_id
def get_surface(self):
if (len(self.objects) < 4):
rospy.warning('There are not enough fiducials to detect surface.')
return None
elif (len(self.objects) > 4):
rospy.warning('There are more than four fiducials for surface, will use first four.')
return
points = [World.objects[0].position, World.objects[1].position,
World.objects[2].position, World.objects[3].position]
s = Surface(points)
return s
def receive_table_marker(self, marker):
'''Callback function for markers to determine table'''
if (marker.type == Marker.LINE_STRIP):
if (len(marker.points) == 6):
rospy.loginfo('Received a TABLE marker.')
xmin = marker.points[0].x
ymin = marker.points[0].y
xmax = marker.points[2].x
ymax = marker.points[2].y
depth = xmax - xmin
width = ymax - ymin
pose = Pose(marker.pose.position, marker.pose.orientation)
pose.position.x = pose.position.x + xmin + depth / 2
pose.position.y = pose.position.y + ymin + width / 2
dimensions = Vector3(depth, width, 0.01)
self.surface = World._get_surface_marker(pose, dimensions)
self._im_server.insert(self.surface,
self.marker_feedback_cb)
self._im_server.applyChanges()
def receive_object_info(self, object_list):
'''Callback function to receive object info'''
self._lock.acquire()
rospy.loginfo('Received recognized object list.')
if (len(object_list.graspable_objects) > 0):
for i in range(len(object_list.graspable_objects)):
models = object_list.graspable_objects[i].potential_models
if (len(models) > 0):
object_pose = None
best_confidence = 0.0
for j in range(len(models)):
if (best_confidence < models[j].confidence):
object_pose = models[j].pose.pose
best_confidence = models[j].confidence
if (object_pose != None):
rospy.logwarn('Adding the recognized object ' +
'with most confident model.')
self._add_new_object(object_pose,
Vector3(0.2, 0.2, 0.2), i,
object_list.meshes[i])
else:
rospy.logwarn('... this is not a recognition result, ' +
'it is probably just segmentation.')
cluster = object_list.graspable_objects[i].cluster
bbox = self._bb_service(cluster)
cluster_pose = bbox.pose.pose
if (cluster_pose != None):
rospy.loginfo('Adding unrecognized object with pose:' +
World.pose_to_string(cluster_pose) + '\n' +
'In ref frame' + str(bbox.pose.header.frame_id))
self._add_new_object(cluster_pose, bbox.box_dims, i)
else:
rospy.logwarn('... but the list was empty.')
self._lock.release()
@staticmethod
def get_pose_from_transform(transform):
'''Returns pose for transformation matrix'''
pos = transform[:3, 3].copy()
rot = tf.transformations.quaternion_from_matrix(transform)
return Pose(Point(pos[0], pos[1], pos[2]),
Quaternion(rot[0], rot[1], rot[2], rot[3]))
@staticmethod
def get_matrix_from_pose(pose):
'''Returns the transformation matrix for given pose'''
rotation = [pose.orientation.x, pose.orientation.y,
pose.orientation.z, pose.orientation.w]
transformation = tf.transformations.quaternion_matrix(rotation)
position = [pose.position.x, pose.position.y, pose.position.z]
transformation[:3, 3] = position
return transformation
@staticmethod
def get_absolute_pose(arm_state):
'''Returns absolute pose of an end effector state'''
if (arm_state.refFrame == ArmState.OBJECT):
arm_state_copy = ArmState(arm_state.refFrame,
Pose(arm_state.ee_pose.position,
arm_state.ee_pose.orientation),
arm_state.joint_pose[:],
arm_state.refFrameObject)
World.convert_ref_frame(arm_state_copy, ArmState.ROBOT_BASE)
return arm_state_copy.ee_pose
else:
return arm_state.ee_pose
@staticmethod
def _get_mesh_marker(marker, mesh):
'''Function that generated a marker from a mesh'''
marker.type = Marker.TRIANGLE_LIST
index = 0
marker.scale = Vector3(1.0, 1.0, 1.0)
while (index + 2 < len(mesh.triangles)):
if ((mesh.triangles[index] < len(mesh.vertices))
and (mesh.triangles[index + 1] < len(mesh.vertices))
and (mesh.triangles[index + 2] < len(mesh.vertices))):
marker.points.append(mesh.vertices[mesh.triangles[index]])
marker.points.append(mesh.vertices[mesh.triangles[index + 1]])
marker.points.append(mesh.vertices[mesh.triangles[index + 2]])
index += 3
else:
rospy.logerr('Mesh contains invalid triangle!')
break
return marker
def _add_new_object(self, pose, dimensions, id=None, mesh=None):
'''Function to add new objects'''
dist_threshold = 0.02
to_remove = None
for i in range(len(World.objects)):
if (World.pose_distance(World.objects[i].object.pose, pose)
< dist_threshold):
rospy.loginfo('Previously detected object at the same' +
'location, will not add this object.')
return False
n_objects = len(World.objects)
World.objects.append(WorldObject(pose, n_objects,
dimensions, id))
int_marker = self._get_object_marker(len(World.objects) - 1)
World.objects[-1].int_marker = int_marker
self._im_server.insert(int_marker, self.marker_feedback_cb)
self._im_server.applyChanges()
World.objects[-1].menu_handler.apply(self._im_server,
int_marker.name)
self._im_server.applyChanges()
return True
def _remove_object(self, to_remove):
'''Function to remove object by index'''
obj = World.objects.pop(to_remove)
rospy.loginfo('Removing object ' + obj.int_marker.name)
self._im_server.erase(obj.int_marker.name)
self._im_server.applyChanges()
def _remove_surface(self):
'''Function to request removing surface'''
rospy.loginfo('Removing surface')
self._im_server.erase('surface')
self._im_server.applyChanges()
self.surface = None
def _get_object_marker(self, index, mesh=None):
'''Generate a marker for world objects'''
int_marker = InteractiveMarker()
int_marker.name = World.objects[index].get_name()
int_marker.header.frame_id = 'base_link'
int_marker.pose = World.objects[index].object.pose
int_marker.scale = 1
button_control = InteractiveMarkerControl()
button_control.interaction_mode = InteractiveMarkerControl.BUTTON
button_control.always_visible = True
object_marker = Marker(type=Marker.CUBE, id=index,
lifetime=rospy.Duration(2),
scale=World.objects[index].object.dimensions,
header=Header(frame_id='base_link'),
color=ColorRGBA(0.2, 0.8, 0.0, 0.6),
pose=World.objects[index].object.pose)
if (mesh != None):
object_marker = World._get_mesh_marker(object_marker, mesh)
button_control.markers.append(object_marker)
text_pos = Point()
text_pos.x = World.objects[index].object.pose.position.x
text_pos.y = World.objects[index].object.pose.position.y
text_pos.z = (World.objects[index].object.pose.position.z +
World.objects[index].object.dimensions.z / 2 + 0.06)
button_control.markers.append(Marker(type=Marker.TEXT_VIEW_FACING,
id=index, scale=Vector3(0, 0, 0.03),
text=int_marker.name, color=ColorRGBA(0.0, 0.0, 0.0, 0.5),
header=Header(frame_id='base_link'),
pose=Pose(text_pos, Quaternion(0, 0, 0, 1))))
int_marker.controls.append(button_control)
return int_marker
@staticmethod
def _get_surface_marker(pose, dimensions):
''' Function that generates a surface marker'''
int_marker = InteractiveMarker()
int_marker.name = 'surface'
int_marker.header.frame_id = 'base_link'
int_marker.pose = pose
int_marker.scale = 1
button_control = InteractiveMarkerControl()
button_control.interaction_mode = InteractiveMarkerControl.BUTTON
button_control.always_visible = True
object_marker = Marker(type=Marker.CUBE, id=2000,
lifetime=rospy.Duration(2),
scale=dimensions,
header=Header(frame_id='base_link'),
color=ColorRGBA(0.8, 0.0, 0.4, 0.4),
pose=pose)
button_control.markers.append(object_marker)
text_pos = Point()
position = pose.position
dimensions = dimensions
text_pos.x = position.x + dimensions.x / 2 - 0.06
text_pos.y = position.y - dimensions.y / 2 + 0.06
text_pos.z = position.z + dimensions.z / 2 + 0.06
text_marker = Marker(type=Marker.TEXT_VIEW_FACING, id=2001,
scale=Vector3(0, 0, 0.03), text=int_marker.name,
color=ColorRGBA(0.0, 0.0, 0.0, 0.5),
header=Header(frame_id='base_link'),
pose=Pose(text_pos, Quaternion(0, 0, 0, 1)))
button_control.markers.append(text_marker)
int_marker.controls.append(button_control)
return int_marker
@staticmethod
def get_frame_list():
'''Function that returns the list of ref. frames'''
objects = []
for i in range(len(World.objects)):
objects.append(World.objects[i].object)
return objects
@staticmethod
def has_objects():
'''Function that checks if there are any objects'''
return len(World.objects) > 0
@staticmethod
def get_ref_from_name(ref_name):
'''Ref. frame type from ref. frame name'''
if ref_name == 'base_link':
return ArmState.ROBOT_BASE
else:
return ArmState.OBJECT
@staticmethod
def convert_ref_frame(arm_frame, ref_frame, ref_frame_obj=Object()):
'''Transforms an arm frame to a new ref. frame'''
if ref_frame == ArmState.ROBOT_BASE:
if (arm_frame.refFrame == ArmState.ROBOT_BASE):
pass
#rospy.logwarn('No reference frame transformations ' +
#'needed (both absolute).')
elif (arm_frame.refFrame == ArmState.OBJECT):
abs_ee_pose = World.transform(arm_frame.ee_pose,
arm_frame.refFrameObject.name, 'base_link')
arm_frame.ee_pose = abs_ee_pose
arm_frame.refFrame = ArmState.ROBOT_BASE
arm_frame.refFrameObject = Object()
else:
rospy.logerr('Unhandled reference frame conversion:' +
str(arm_frame.refFrame) + ' to ' + str(ref_frame))
elif ref_frame == ArmState.OBJECT:
if (arm_frame.refFrame == ArmState.ROBOT_BASE):
rel_ee_pose = World.transform(arm_frame.ee_pose,
'base_link', ref_frame_obj.name)
arm_frame.ee_pose = rel_ee_pose
arm_frame.refFrame = ArmState.OBJECT
arm_frame.refFrameObject = ref_frame_obj
elif (arm_frame.refFrame == ArmState.OBJECT):
if (arm_frame.refFrameObject.name == ref_frame_obj.name):
pass
#rospy.logwarn('No reference frame transformations ' +
#'needed (same object).')
else:
rel_ee_pose = World.transform(arm_frame.ee_pose,
arm_frame.refFrameObject.name, ref_frame_obj.name)
arm_frame.ee_pose = rel_ee_pose
arm_frame.refFrame = ArmState.OBJECT
arm_frame.refFrameObject = ref_frame_obj
else:
rospy.logerr('Unhandled reference frame conversion:' +
str(arm_frame.refFrame) + ' to ' + str(ref_frame))
return arm_frame
@staticmethod
def has_object(object_name):
'''Checks if the world contains the object'''
for obj in World.objects:
if obj.object.name == object_name:
return True
return False
@staticmethod
def is_frame_valid(object_name):
'''Checks if the frame is valid for transforms'''
return (object_name == 'base_link') or World.has_object(object_name)
@staticmethod
def transform(pose, from_frame, to_frame):
''' Function to transform a pose between two ref. frames
if there is a TF exception or object does not exist it
will return the pose back without any transforms'''
if World.is_frame_valid(from_frame) and World.is_frame_valid(to_frame):
pose_stamped = PoseStamped()
try:
common_time = World.tf_listener.getLatestCommonTime(from_frame,
to_frame)
pose_stamped.header.stamp = common_time
pose_stamped.header.frame_id = from_frame
pose_stamped.pose = pose
rel_ee_pose = World.tf_listener.transformPose(to_frame,
pose_stamped)
return rel_ee_pose.pose
except tf.Exception:
rospy.logerr('TF exception during transform.')
return pose
except rospy.ServiceException:
rospy.logerr('Exception during transform.')
return pose
else:
rospy.logwarn('One of the frame objects might not exist: ' +
from_frame + ' or ' + to_frame)
return pose
@staticmethod
def pose_to_string(pose):
'''For printing a pose to stdout'''
return ('Position: ' + str(pose.position.x) + ", " +
str(pose.position.y) + ', ' + str(pose.position.z) + '\n' +
'Orientation: ' + str(pose.orientation.x) + ", " +
str(pose.orientation.y) + ', ' + str(pose.orientation.z) +
', ' + str(pose.orientation.w) + '\n')
def _publish_tf_pose(self, pose, name, parent):
''' Publishes a TF for object pose'''
if (pose != None):
pos = (pose.position.x, pose.position.y, pose.position.z)
rot = (pose.orientation.x, pose.orientation.y,
pose.orientation.z, pose.orientation.w)
self._tf_broadcaster.sendTransform(pos, rot,
rospy.Time.now(), name, parent)
def update_object_pose(self):
''' Function to externally update an object pose'''
Response.perform_gaze_action(GazeGoal.LOOK_DOWN)
while (Response.gaze_client.get_state() == GoalStatus.PENDING or
Response.gaze_client.get_state() == GoalStatus.ACTIVE):
time.sleep(0.1)
if (Response.gaze_client.get_state() != GoalStatus.SUCCEEDED):
rospy.logerr('Could not look down to take table snapshot')
return False
rospy.loginfo('Looking at table now.')
goal = UserCommandGoal(UserCommandGoal.RESET, False)
self._object_action_client.send_goal(goal)
while (self._object_action_client.get_state() == GoalStatus.ACTIVE or
self._object_action_client.get_state() == GoalStatus.PENDING):
time.sleep(0.1)
rospy.loginfo('Object recognition has been reset.')
rospy.loginfo('STATUS: ' +
self._object_action_client.get_goal_status_text())
self._reset_objects()
if (self._object_action_client.get_state() != GoalStatus.SUCCEEDED):
rospy.logerr('Could not reset recognition.')
return False
# Do segmentation
goal = UserCommandGoal(UserCommandGoal.SEGMENT, False)
self._object_action_client.send_goal(goal)
while (self._object_action_client.get_state() == GoalStatus.ACTIVE or
self._object_action_client.get_state() == GoalStatus.PENDING):
time.sleep(0.1)
rospy.loginfo('Table segmentation is complete.')
rospy.loginfo('STATUS: ' +
self._object_action_client.get_goal_status_text())
if (self._object_action_client.get_state() != GoalStatus.SUCCEEDED):
rospy.logerr('Could not segment.')
return False
# Do recognition
goal = UserCommandGoal(UserCommandGoal.RECOGNIZE, False)
self._object_action_client.send_goal(goal)
while (self._object_action_client.get_state() == GoalStatus.ACTIVE or
self._object_action_client.get_state() == GoalStatus.PENDING):
time.sleep(0.1)
rospy.loginfo('Objects on the table have been recognized.')
rospy.loginfo('STATUS: ' +
self._object_action_client.get_goal_status_text())
# Record the result
if (self._object_action_client.get_state() == GoalStatus.SUCCEEDED):
wait_time = 0
total_wait_time = 5
while (not World.has_objects() and wait_time < total_wait_time):
time.sleep(0.1)
wait_time += 0.1
if (not World.has_objects()):
rospy.logerr('Timeout waiting for a recognition result.')
return False
else:
rospy.loginfo('Got the object list.')
return True
else:
rospy.logerr('Could not recognize.')
return False
def clear_all_objects(self):
'''Removes all objects from the world'''
goal = UserCommandGoal(UserCommandGoal.RESET, False)
self._object_action_client.send_goal(goal)
while (self._object_action_client.get_state() == GoalStatus.ACTIVE or
self._object_action_client.get_state() == GoalStatus.PENDING):
time.sleep(0.1)
rospy.loginfo('Object recognition has been reset.')
rospy.loginfo('STATUS: ' +
self._object_action_client.get_goal_status_text())
if (self._object_action_client.get_state() == GoalStatus.SUCCEEDED):
rospy.loginfo('Successfully reset object localization pipeline.')
self._reset_objects()
self._remove_surface()
def get_nearest_object(self, arm_pose):
'''Gives a pointed to the nearest object'''
dist_threshold = 0.4
def chObj(cur, obj):
dist = World.pose_distance(obj.object.pose, arm_pose)
return (dist, obj.object) if (dist < cur[0]) else cur
return reduce(chObj, World.objects, (dist_threshold, None))[1]
@staticmethod
def pose_distance(pose1, pose2, is_on_table=True):
'''Distance between two world poses'''
if pose1 == [] or pose2 == []:
return 0.0
else:
if (is_on_table):
arr1 = array([pose1.position.x, pose1.position.y])
arr2 = array([pose2.position.x, pose2.position.y])
else:
arr1 = array([pose1.position.x,
pose1.position.y, pose1.position.z])
arr2 = array([pose2.position.x,
pose2.position.y, pose2.position.z])
dist = norm(arr1 - arr2)
if dist < 0.0001:
dist = 0
return dist
def marker_feedback_cb(self, feedback):
'''Callback for when feedback from a marker is received'''
if feedback.event_type == InteractiveMarkerFeedback.BUTTON_CLICK:
rospy.loginfo('Clicked on object ' + str(feedback.marker_name))
rospy.loginfo('Number of objects ' + str(len(World.objects)))
else:
rospy.loginfo('Unknown event' + str(feedback.event_type))
def update(self):
'''Update function called in a loop'''
# Visualize the detected object
is_world_changed = False
self._lock.acquire()
if (World.has_objects()):
to_remove = None
for i in range(len(World.objects)):
self._publish_tf_pose(World.objects[i].object.pose,
World.objects[i].get_name(), 'base_link')
if (World.objects[i].is_removed):
to_remove = i
if to_remove != None:
self._remove_object(to_remove)
is_world_changed = True
self._lock.release()
return is_world_changed
class PublishTf:
def __init__(self):
self.listener = TransformListener()
self.br = TransformBroadcaster()
self.pub_freq = 100.0
self.parent_frame_id = "world"
self.child1_frame_id = "reference1"
self.child2_frame_id = "reference2"
self.child3_frame_id = "reference3"
self.child4_frame_id = "reference4"
rospy.Timer(rospy.Duration(1 / self.pub_freq), self.reference2)
rospy.Timer(rospy.Duration(1 / self.pub_freq), self.reference3)
rospy.Timer(rospy.Duration(1 / self.pub_freq), self.reference4)
rospy.sleep(1.0)
recorder_0 = RecordingManager("all")
recorder_1 = RecordingManager("test1")
recorder_2 = RecordingManager("test2")
recorder_3 = RecordingManager("test3")
recorder_0.start()
recorder_1.start()
self.pub_line(length=1, time=5)
recorder_1.stop()
recorder_2.start()
self.pub_quadrat(length=2, time=10)
recorder_2.stop()
recorder_3.start()
self.pub_circ(radius=2, time=5)
recorder_3.stop()
recorder_0.stop()
def reference2(self, event):
self.check_for_ctrlc()
self.pub_tf(self.child1_frame_id, self.child2_frame_id, [1, 0, 0])
def reference3(self, event):
self.check_for_ctrlc()
self.pub_tf(self.child1_frame_id, self.child3_frame_id, [math.sin(rospy.Time.now().to_sec()), 0, 0])
def reference4(self, event):
self.check_for_ctrlc()
self.pub_tf(self.child1_frame_id, self.child4_frame_id, [math.sin(rospy.Time.now().to_sec()),
math.cos(rospy.Time.now().to_sec()), 0])
def pub_tf(self, parent_frame_id, child1_frame_id, xyz=[0, 0, 0], rpy=[0, 0, 0]):
self.check_for_ctrlc()
self.br.sendTransform((xyz[0], xyz[1], xyz[2]), transformations.quaternion_from_euler(
rpy[0], rpy[1], rpy[2]), rospy.Time.now(), child1_frame_id, parent_frame_id)
def pub_line(self, length=1, time=1):
rospy.loginfo("Line")
rate = rospy.Rate(int(self.pub_freq))
for i in range((int(self.pub_freq * time / 2) + 1)):
t = i / self.pub_freq / time * 2
self.pub_tf(self.parent_frame_id, self.child1_frame_id, [t * length, 0, 0])
rate.sleep()
for i in range((int(self.pub_freq * time / 2) + 1)):
t = i / self.pub_freq / time * 2
self.pub_tf(self.parent_frame_id, self.child1_frame_id, [(1 - t) * length, 0, 0])
rate.sleep()
def pub_circ(self, radius=1, time=1):
rospy.loginfo("Circ")
rate = rospy.Rate(int(self.pub_freq))
for i in range(int(self.pub_freq * time) + 1):
t = i / self.pub_freq / time
self.pub_tf(self.parent_frame_id, self.child1_frame_id, [-radius * math.cos(2 * math.pi * t) + radius,
-radius * math.sin(2 * math.pi * t),
0])
rate.sleep()
def pub_quadrat(self, length=1, time=1):
rospy.loginfo("Quadrat")
rate = rospy.Rate(int(self.pub_freq))
for i in range((int(self.pub_freq * time / 4) + 1)):
t = i / self.pub_freq / time * 4
self.pub_tf(self.parent_frame_id, self.child1_frame_id, [t * length, 0, 0])
rate.sleep()
for i in range((int(self.pub_freq * time / 4) + 1)):
t = i / self.pub_freq / time * 4
self.pub_tf(self.parent_frame_id, self.child1_frame_id, [length, t * length, 0])
rate.sleep()
for i in range((int(self.pub_freq * time / 4) + 1)):
t = i / self.pub_freq / time * 4
self.pub_tf(self.parent_frame_id, self.child1_frame_id, [(1 - t) * length, length, 0])
rate.sleep()
for i in range((int(self.pub_freq * time / 4) + 1)):
t = i / self.pub_freq / time * 4
self.pub_tf(self.parent_frame_id, self.child1_frame_id, [0, (1 - t) * length, 0])
rate.sleep()
@staticmethod
def check_for_ctrlc():
if rospy.is_shutdown():
sys.exit()
class World:
'''Object recognition and localization related stuff'''
tf_listener = None
objects = []
world = None
segmentation_service = rospy.get_param("/pr2_pbd_interaction/tabletop_segmentation_service")
def __init__(self):
if World.tf_listener is None:
World.tf_listener = TransformListener()
self._lock = threading.Lock()
self.surface = None
self._tf_broadcaster = TransformBroadcaster()
self._im_server = InteractiveMarkerServer('world_objects')
self.clear_all_objects()
self.relative_frame_threshold = 0.4
# rospy.Subscriber("ar_pose_marker",
# AlvarMarkers, self.receive_ar_markers)
self.is_looking_for_markers = False
self.marker_dims = Vector3(0.04, 0.04, 0.01)
World.world = self
@staticmethod
def get_world():
if World.world is None:
World.world = World()
return World.world
def _reset_objects(self):
'''Function that removes all objects'''
self._lock.acquire()
for i in range(len(World.objects)):
self._im_server.erase(World.objects[i].int_marker.name)
self._im_server.applyChanges()
if self.surface != None:
self._remove_surface()
#self._im_server.clear()
self._im_server.applyChanges()
World.objects = []
self._lock.release()
def receieve_table_marker(self, marker):
'''Callback function for markers to determine table'''
if (marker.type == Marker.LINE_STRIP):
if (len(marker.points) == 6):
rospy.loginfo('Received a TABLE marker.')
xmin = marker.points[0].x
ymin = marker.points[0].y
xmax = marker.points[2].x
ymax = marker.points[2].y
depth = xmax - xmin
width = ymax - ymin
pose = Pose(marker.pose.position, marker.pose.orientation)
pose.position.x = pose.position.x + xmin + depth / 2
pose.position.y = pose.position.y + ymin + width / 2
dimensions = Vector3(depth, width, 0.01)
self.surface = World._get_surface_marker(pose, dimensions)
self._im_server.insert(self.surface,
self.marker_feedback_cb)
self._im_server.applyChanges()
def receive_ar_markers(self, data):
'''Callback function to receive marker info'''
self._lock.acquire()
if self.is_looking_for_markers:
if len(data.markers) > 0:
rospy.loginfo('Received non-empty marker list.')
for i in range(len(data.markers)):
marker = data.markers[i]
self._add_new_object(marker.pose.pose, self.marker_dims, False, id=marker.id)
self._lock.release()
def receieve_object_info(self, object_list):
'''Callback function to receive object info'''
self._lock.acquire()
rospy.loginfo('Received recognized object list.')
if (len(object_list.graspable_objects) > 0):
for i in range(len(object_list.graspable_objects)):
models = object_list.graspable_objects[i].potential_models
if (len(models) > 0):
object_pose = None
best_confidence = 0.0
for j in range(len(models)):
if (best_confidence < models[j].confidence):
object_pose = models[j].pose.pose
best_confidence = models[j].confidence
if (object_pose != None):
rospy.logwarn('Adding the recognized object ' +
'with most confident model.')
self._add_new_object(object_pose,
Vector3(0.2, 0.2, 0.2), True,
object_list.meshes[i])
else:
rospy.logwarn('... this is not a recognition result, ' +
'it is probably just segmentation.')
cluster = object_list.graspable_objects[i].cluster
bbox = self._bb_service(cluster)
cluster_pose = bbox.pose.pose
if (cluster_pose != None):
rospy.loginfo('Adding unrecognized object with pose:' +
World.pose_to_string(cluster_pose) + '\n' +
'In ref frame' + str(bbox.pose.header.frame_id))
self._add_new_object(cluster_pose, bbox.box_dims,
False)
else:
rospy.logwarn('... but the list was empty.')
self._lock.release()
@staticmethod
def get_pose_from_transform(transform):
'''Returns pose for transformation matrix'''
pos = transform[:3, 3].copy()
rot = tf.transformations.quaternion_from_matrix(transform)
return Pose(Point(pos[0], pos[1], pos[2]),
Quaternion(rot[0], rot[1], rot[2], rot[3]))
@staticmethod
def get_matrix_from_pose(pose):
'''Returns the transformation matrix for given pose'''
rotation = [pose.orientation.x, pose.orientation.y,
pose.orientation.z, pose.orientation.w]
transformation = tf.transformations.quaternion_matrix(rotation)
position = [pose.position.x, pose.position.y, pose.position.z]
transformation[:3, 3] = position
return transformation
@staticmethod
def get_absolute_pose(arm_state):
'''Returns absolute pose of an end effector state'''
if (arm_state.refFrame == ArmState.OBJECT):
arm_state_copy = ArmState(arm_state.refFrame,
Pose(arm_state.ee_pose.position,
arm_state.ee_pose.orientation),
arm_state.joint_pose[:],
arm_state.refFrameObject)
World.convert_ref_frame(arm_state_copy, ArmState.ROBOT_BASE)
return arm_state_copy.ee_pose
else:
return arm_state.ee_pose
@staticmethod
def get_most_similar_obj(ref_object, ref_frame_list, threshold=0.075):
'''Finds the most similar object in the world'''
best_dist = 10000
chosen_obj_index = -1
for i in range(len(ref_frame_list)):
dist = World.object_dissimilarity(ref_frame_list[i], ref_object)
if (dist < best_dist):
best_dist = dist
chosen_obj_index = i
if chosen_obj_index == -1:
rospy.logwarn('Did not find a similar object..')
return None
else:
print 'Object dissimilarity is --- ', best_dist
if best_dist > threshold:
rospy.logwarn('Found some objects, but not similar enough.')
return None
else:
rospy.loginfo('Most similar to object '
+ ref_frame_list[chosen_obj_index].name)
return ref_frame_list[chosen_obj_index]
@staticmethod
def get_map_of_most_similar_obj(object_list, ref_frame_list, threshold=0.075):
if None in object_list:
object_list.remove(None)
if len(object_list) == 0 or len(ref_frame_list) == 0:
return None
markers_dict = {}
marker_names = []
for obj in object_list:
object_name = obj.name
if object_name.startswith("marker"):
marker_names.append(object_name)
found_match = False
for ref_frame in ref_frame_list:
if ref_frame.name == object_name:
markers_dict[object_name] = ref_frame
found_match = True
break
if not found_match:
return None
ref_frame_list = [x for x in ref_frame_list if x.name not in marker_names]
object_list = [x for x in object_list if x.name not in marker_names]
if len(object_list) == 0 or len(ref_frame_list) == 0:
return markers_dict if len(markers_dict) > 0 else None
orderings = []
World.permute(object_list, orderings)
costs = []
assignments = []
for ordering in orderings:
cur_cost = 0
cur_ref_frame_list = ref_frame_list[:]
cur_assignment = []
for object in ordering:
most_similar_object = World.get_most_similar_obj(object, cur_ref_frame_list, threshold)
if most_similar_object is None:
return None
cur_ref_frame_list.remove(most_similar_object)
cur_assignment.append(most_similar_object)
cur_cost += World.object_dissimilarity(object, most_similar_object)
costs.append(cur_cost)
assignments.append(cur_assignment)
min_cost, min_idx = min((val, idx) for (idx, val) in enumerate(costs))
names = [x.name for x in orderings[min_idx]]
object_dict = dict(zip(names, assignments[min_idx]))
object_dict.update(markers_dict)
return object_dict
@staticmethod
def permute(a, results):
if len(a) == 1:
results.insert(len(results), a)
else:
for i in range(0, len(a)):
element = a[i]
a_copy = [a[j] for j in range(0, len(a)) if j != i]
subresults = []
World.permute(a_copy, subresults)
for subresult in subresults:
result = [element] + subresult
results.insert(len(results), result)
@staticmethod
def _get_mesh_marker(marker, mesh):
'''Function that generated a marker from a mesh'''
marker.type = Marker.TRIANGLE_LIST
index = 0
marker.scale = Vector3(1.0, 1.0, 1.0)
while (index + 2 < len(mesh.triangles)):
if ((mesh.triangles[index] < len(mesh.vertices))
and (mesh.triangles[index + 1] < len(mesh.vertices))
and (mesh.triangles[index + 2] < len(mesh.vertices))):
marker.points.append(mesh.vertices[mesh.triangles[index]])
marker.points.append(mesh.vertices[mesh.triangles[index + 1]])
marker.points.append(mesh.vertices[mesh.triangles[index + 2]])
index += 3
else:
rospy.logerr('Mesh contains invalid triangle!')
break
return marker
def _add_new_object(self, pose, dimensions, is_recognized, mesh=None, id=None):
'''Function to add new objects'''
dist_threshold = 0.02
to_remove = None
if (is_recognized):
# Temporary HACK for testing.
# Will remove all recognition completely if this works.
return False
# Check if there is already an object
for i in range(len(World.objects)):
distance = World.pose_distance(World.objects[i].object.pose,
pose)
if (distance < dist_threshold):
if (World.objects[i].is_recognized):
rospy.loginfo('Previously recognized object at ' +
'the same location, will not add this object.')
return False
else:
rospy.loginfo('Previously unrecognized object at ' +
'the same location, will replace it with the ' +
'recognized object.')
to_remove = i
break
if (to_remove != None):
self._remove_object(to_remove)
n_objects = len(World.objects)
new_object = WorldObject(pose, n_objects,
dimensions, is_recognized)
if id is not None:
new_object.assign_name("marker" + str(id))
new_object.is_marker = True
World.objects.append(new_object)
int_marker = self._get_object_marker(len(World.objects) - 1, mesh)
World.objects[-1].int_marker = int_marker
self._im_server.insert(int_marker, self.marker_feedback_cb)
self._im_server.applyChanges()
World.objects[-1].menu_handler.apply(self._im_server,
int_marker.name)
self._im_server.applyChanges()
return True
else:
for i in range(len(World.objects)):
if (World.pose_distance(World.objects[i].object.pose, pose)
< dist_threshold):
rospy.loginfo('Previously detected object at the same' +
'location, will not add this object.')
return False
if id is not None and World.objects[i].get_name() == "marker" + str(id):
rospy.loginfo('Previously added marker with the same id, will not add this object.')
return False
n_objects = len(World.objects)
new_object = WorldObject(pose, n_objects,
dimensions, is_recognized)
if id is not None:
new_object.assign_name("marker" + str(id))
new_object.is_marker = True
World.objects.append(new_object)
int_marker = self._get_object_marker(len(World.objects) - 1)
World.objects[-1].int_marker = int_marker
self._im_server.insert(int_marker, self.marker_feedback_cb)
self._im_server.applyChanges()
World.objects[-1].menu_handler.apply(self._im_server,
int_marker.name)
self._im_server.applyChanges()
return True
def _add_new_marker(self, id, pose):
'''Function to add new markers'''
#dist_threshold = 0.01
#to_remove = None
#for i in range(len(World.markers)):
# if (World.pose_distance(World.markers[i].pose, pose)
# < dist_threshold):
# rospy.loginfo('Previously detected marker at the same' +
# 'location, will not add this marker.')
# return False
#n_markers = len(World.markers)
#World.markers.append(WorldMarker(id, pose))
#int_marker = self._get_object_marker(len(World.objects) - 1)
#World.markers[-1].int_marker = int_marker
#self._im_server.insert(int_marker, self.marker_feedback_cb)
#self._im_server.applyChanges()
#World.markers[-1].menu_handler.apply(self._im_server,
# int_marker.name)
#self._im_server.applyChanges()
return True
def _remove_object(self, to_remove):
'''Function to remove object by index'''
obj = World.objects.pop(to_remove)
rospy.loginfo('Removing object ' + obj.int_marker.name)
self._im_server.erase(obj.int_marker.name)
self._im_server.applyChanges()
# if (obj.is_recognized):
# for i in range(len(World.objects)):
# if ((World.objects[i].is_recognized)
# and World.objects[i].index>obj.index):
# World.objects[i].decrease_index()
# self.n_recognized -= 1
# else:
# for i in range(len(World.objects)):
# if ((not World.objects[i].is_recognized) and
# World.objects[i].index>obj.index):
# World.objects[i].decrease_index()
# self.n_unrecognized -= 1
def _remove_surface(self):
'''Function to request removing surface'''
rospy.loginfo('Removing surface')
self._im_server.erase('surface')
self._im_server.applyChanges()
self.surface = None
def _get_object_reachability_marker(self, world_object):
radius = self.relative_frame_threshold
pointsList = []
nx = 8
ny = 8
pointsList.append(Point(0, 0, radius))
pointsList.append(Point(0, 0, -radius))
for x in range(nx):
theta = 2 * pi * (x * 1.0 / nx)
for y in range(1, ny):
phi = pi * (y * 1.0 / ny)
destx = radius * cos(theta) * sin(phi)
desty = radius * sin(theta) * sin(phi)
destz = radius * cos(phi)
pointsList.append(Point(destx, desty, destz))
id = abs(hash(world_object.get_name() + "_reachability")) % (10 ** 8)
marker = Marker(type=Marker.SPHERE_LIST, id=id,
lifetime=rospy.Duration(nsecs=10 ** 8),
scale=Vector3(0.01, 0.01, 0.01),
points=set(pointsList),
header=Header(frame_id='base_link'),
color=ColorRGBA(1, 1, 1, 0.5),
pose=world_object.object.pose)
return marker
def _get_object_marker(self, index, mesh=None):
'''Generate a marker for world objects'''
int_marker = InteractiveMarker()
int_marker.name = World.objects[index].get_name()
int_marker.header.frame_id = 'base_link'
int_marker.pose = World.objects[index].object.pose
int_marker.scale = 1
button_control = InteractiveMarkerControl()
button_control.interaction_mode = InteractiveMarkerControl.BUTTON
button_control.always_visible = True
color = WorldObject.default_color if not World.objects[index].is_fake else WorldObject.fake_color
object_marker = Marker(type=Marker.CUBE, id=index,
lifetime=rospy.Duration(2),
scale=World.objects[index].object.dimensions,
header=Header(frame_id='base_link'),
color=color,
pose=Pose(Point(0, 0, 0), Quaternion(0, 0, 0, 1)))
if (mesh != None):
object_marker = World._get_mesh_marker(object_marker, mesh)
button_control.markers.append(object_marker)
text_pos = Point()
text_pos.x = 0
text_pos.y = 0
text_pos.z = World.objects[index].object.dimensions.z / 2 + 0.06
button_control.markers.append(Marker(type=Marker.TEXT_VIEW_FACING,
id=index, scale=Vector3(0.05, 0.05, 0.05),
text=int_marker.name, color=ColorRGBA(0.0, 0.0, 0.0, 0.5),
header=Header(frame_id='base_link'),
pose=Pose(text_pos, Quaternion(0, 0, 0, 1))))
int_marker.controls.append(button_control)
return int_marker
@staticmethod
def _get_surface_marker(pose, dimensions):
''' Function that generates a surface marker'''
int_marker = InteractiveMarker()
int_marker.name = 'surface'
int_marker.header.frame_id = 'base_link'
int_marker.pose = pose
int_marker.scale = 1
button_control = InteractiveMarkerControl()
button_control.interaction_mode = InteractiveMarkerControl.BUTTON
button_control.always_visible = True
object_marker = Marker(type=Marker.CUBE, id=2000,
lifetime=rospy.Duration(2),
scale=dimensions,
header=Header(frame_id='base_link'),
color=ColorRGBA(0.8, 0.0, 0.4, 0.4),
pose=Pose(Point(0, 0, 0), Quaternion(0, 0, 0, 1)))
button_control.markers.append(object_marker)
text_pos = Point()
dimensions = dimensions
text_pos.x = dimensions.x / 2 - 0.06
text_pos.y = - dimensions.y / 2 + 0.06
text_pos.z = dimensions.z / 2 + 0.06
text_marker = Marker(type=Marker.TEXT_VIEW_FACING, id=2001,
scale=Vector3(0.05, 0.05, 0.05), text=int_marker.name,
color=ColorRGBA(0.0, 0.0, 0.0, 0.5),
header=Header(frame_id='base_link'),
pose=Pose(text_pos, Quaternion(0, 0, 0, 1)))
button_control.markers.append(text_marker)
int_marker.controls.append(button_control)
return int_marker
def get_frame_list(self):
'''Function that returns the list of ref. frames'''
self._lock.acquire()
objects = []
for i in range(len(World.objects)):
objects.append(World.objects[i].object)
self._lock.release()
return objects
@staticmethod
def has_objects():
'''Function that checks if there are any objects'''
return len(World.objects) > 0
@staticmethod
def has_marker_objects():
'''Function that checks if there are any markers'''
for o in World.objects:
if o.is_marker:
return True
return False
@staticmethod
def has_non_marker_objects():
'''Function that checks if there are any objects'''
for o in World.objects:
if not o.is_marker:
return True
return False
@staticmethod
def object_dissimilarity(obj1, obj2):
'''Distance between two objects'''
dims1 = obj1.dimensions
dims2 = obj2.dimensions
return norm(array([dims1.x, dims1.y, dims1.z]) -
array([dims2.x, dims2.y, dims2.z]))
@staticmethod
def get_ref_from_name(ref_name):
'''Ref. frame type from ref. frame name'''
if ref_name == 'base_link':
return ArmState.ROBOT_BASE
else:
return ArmState.OBJECT
@staticmethod
def convert_ref_frame(arm_frame, ref_frame, ref_frame_obj=Object()):
'''Transforms an arm frame to a new ref. frame'''
if ref_frame == ArmState.ROBOT_BASE:
if (arm_frame.refFrame == ArmState.ROBOT_BASE):
rospy.logwarn('No reference frame transformations ' +
'needed (both absolute).')
elif (arm_frame.refFrame == ArmState.OBJECT):
abs_ee_pose = World.transform(arm_frame.ee_pose,
arm_frame.refFrameObject.name, 'base_link')
arm_frame.ee_pose = abs_ee_pose
arm_frame.refFrame = ArmState.ROBOT_BASE
arm_frame.refFrameObject = Object()
else:
rospy.logerr('Unhandled reference frame conversion:' +
str(arm_frame.refFrame) + ' to ' + str(ref_frame))
elif ref_frame == ArmState.OBJECT:
if (arm_frame.refFrame == ArmState.ROBOT_BASE):
rel_ee_pose = World.transform(arm_frame.ee_pose,
'base_link', ref_frame_obj.name)
arm_frame.ee_pose = rel_ee_pose
arm_frame.refFrame = ArmState.OBJECT
arm_frame.refFrameObject = ref_frame_obj
elif (arm_frame.refFrame == ArmState.OBJECT):
if (arm_frame.refFrameObject.name == ref_frame_obj.name):
rospy.logwarn('No reference frame transformations ' +
'needed (same object).')
else:
rel_ee_pose = World.transform(arm_frame.ee_pose,
arm_frame.refFrameObject.name, ref_frame_obj.name)
arm_frame.ee_pose = rel_ee_pose
arm_frame.refFrame = ArmState.OBJECT
arm_frame.refFrameObject = ref_frame_obj
else:
rospy.logerr('Unhandled reference frame conversion:' +
str(arm_frame.refFrame) + ' to ' + str(ref_frame))
return arm_frame
@staticmethod
def has_object(object_name):
'''Checks if the world contains the object'''
for obj in World.objects:
if obj.object.name == object_name:
return True
return False
@staticmethod
def is_frame_valid(object_name):
'''Checks if the frame is valid for transforms'''
return (object_name == 'base_link') or World.has_object(object_name)
@staticmethod
def transform(pose, from_frame, to_frame):
''' Function to transform a pose between two ref. frames
if there is a TF exception or object does not exist it
will return the pose back without any transforms'''
if World.is_frame_valid(from_frame) and World.is_frame_valid(to_frame):
pose_stamped = PoseStamped()
try:
common_time = World.tf_listener.getLatestCommonTime(from_frame,
to_frame)
pose_stamped.header.stamp = common_time
pose_stamped.header.frame_id = from_frame
pose_stamped.pose = pose
rel_ee_pose = World.tf_listener.transformPose(to_frame,
pose_stamped)
return rel_ee_pose.pose
except tf.Exception:
rospy.logerr('TF exception during transform.')
return pose
except rospy.ServiceException:
rospy.logerr('Exception during transform.')
return pose
else:
# rospy.logwarn('One of the frame objects might not exist: ' +
# from_frame + ' or ' + to_frame)
return pose
@staticmethod
def pose_to_string(pose):
'''For printing a pose to stdout'''
return ('Position: ' + str(pose.position.x) + ", " +
str(pose.position.y) + ', ' + str(pose.position.z) + '\n' +
'Orientation: ' + str(pose.orientation.x) + ", " +
str(pose.orientation.y) + ', ' + str(pose.orientation.z) +
', ' + str(pose.orientation.w) + '\n')
def _publish_tf_pose(self, pose, name, parent):
''' Publishes a TF for object pose'''
if (pose != None):
pos = (pose.position.x, pose.position.y, pose.position.z)
rot = (pose.orientation.x, pose.orientation.y,
pose.orientation.z, pose.orientation.w)
self._tf_broadcaster.sendTransform(pos, rot,
rospy.Time.now(), name, parent)
def update_object_pose(self):
''' Function to externally update an object pose'''
rospy.loginfo("waiting for segmentation service")
rospy.wait_for_service(self.segmentation_service)
try:
get_segmentation = rospy.ServiceProxy(self.segmentation_service, TabletopSegmentation)
resp = get_segmentation()
rospy.loginfo("Adding landmarks")
self._reset_objects()
# add the table
xmin = resp.table.x_min
ymin = resp.table.y_min
xmax = resp.table.x_max
ymax = resp.table.y_max
depth = xmax - xmin
width = ymax - ymin
pose = resp.table.pose.pose
pose.position.x = pose.position.x + xmin + depth / 2
pose.position.y = pose.position.y + ymin + width / 2
dimensions = Vector3(depth, width, 0.01)
self.surface = World._get_surface_marker(pose, dimensions)
self._im_server.insert(self.surface,
self.marker_feedback_cb)
self._im_server.applyChanges()
for cluster in resp.clusters:
points = cluster.points
if (len(points) == 0):
return Point(0, 0, 0)
[minX, maxX, minY, maxY, minZ, maxZ] = [
points[0].x, points[0].x, points[0].y, points[0].y,
points[0].z, points[0].z]
for pt in points:
minX = min(minX, pt.x)
minY = min(minY, pt.y)
minZ = min(minZ, pt.z)
maxX = max(maxX, pt.x)
maxY = max(maxY, pt.y)
maxZ = max(maxZ, pt.z)
self._add_new_object(Pose(Point((minX + maxX) / 2, (minY + maxY) / 2,
(minZ + maxZ) / 2), Quaternion(0, 0, 0, 1)),
Point(maxX - minX, maxY - minY, maxZ - minZ), False)
return True
except rospy.ServiceException, e:
print "Call to segmentation service failed: %s" % e
return False
class MyAMCL:
def __init__(self):
self.initialized = False
rospy.init_node('my_amcl')
print "MY AMCL initialized"
# todo make this static
self.n_particles = 100
self.alpha1 = 0.2
self.alpha2 = 0.2
self.alpha3 = 0.2
self.alpha4 = 0.2
self.d_thresh = 0.2
self.a_thresh = math.pi/6
self.z_hit = 0.5
self.z_rand = 0.5
self.sigma_hit = 0.2
self.laser_max_distance = 2.0
self.laser_subscriber = rospy.Subscriber("scan", LaserScan, self.scan_received);
self.tf_listener = TransformListener()
self.tf_broadcaster = TransformBroadcaster()
self.particle_pub = rospy.Publisher("particlecloud", PoseArray)
self.particle_cloud = []
self.last_transform_valid = False
self.particle_cloud_initialized = False
self.current_odom_xy_theta = []
# request the map
rospy.wait_for_service("static_map")
static_map = rospy.ServiceProxy("static_map", GetMap)
try:
self.map = static_map().map
except:
print "error receiving map"
self.create_occupancy_field()
self.initialized = True
def create_occupancy_field(self):
X = np.zeros((self.map.info.width*self.map.info.height,2))
total_occupied = 0
curr = 0
for i in range(self.map.info.width):
for j in range(self.map.info.height):
# occupancy grids are stored in row major order, if you go through this right, you might be able to use curr
ind = i + j*self.map.info.width
if self.map.data[ind] > 0:
total_occupied += 1
X[curr,0] = float(i)
X[curr,1] = float(j)
curr += 1
O = np.zeros((total_occupied,2))
curr = 0
for i in range(self.map.info.width):
for j in range(self.map.info.height):
# occupancy grids are stored in row major order, if you go through this right, you might be able to use curr
ind = i + j*self.map.info.width
if self.map.data[ind] > 0:
O[curr,0] = float(i)
O[curr,1] = float(j)
curr += 1
t = time.time()
nbrs = NearestNeighbors(n_neighbors=1,algorithm="ball_tree").fit(O)
distances, indices = nbrs.kneighbors(X)
print time.time() -t
closest_occ = {}
curr = 0
for i in range(self.map.info.width):
for j in range(self.map.info.height):
ind = i + j*self.map.info.width
closest_occ[ind] = distances[curr]*self.map.info.resolution
curr += 1
# this is a bit adhoc, could probably integrate into an internal map structure
self.closest_occ = closest_occ
def update_robot_pose(self):
# 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):
new_odom_xy_theta = MyAMCL.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 = MyAMCL.angle_diff(math.atan2(delta[1], delta[0]), old_odom_xy_theta[2])
delta_rot2 = MyAMCL.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(MyAMCL.angle_diff(delta_rot1,0.0)), math.fabs(MyAMCL.angle_diff(delta_rot1, math.pi)));
delta_rot2_noise = min(math.fabs(MyAMCL.angle_diff(delta_rot2,0.0)), math.fabs(MyAMCL.angle_diff(delta_rot2, math.pi)));
for sample in self.particle_cloud:
# Sample pose differences
delta_rot1_hat = MyAMCL.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 = MyAMCL.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 get_map_index(self,x,y):
x_coord = int((x - self.map.info.origin.position.x)/self.map.info.resolution)
y_coord = int((y - self.map.info.origin.position.y)/self.map.info.resolution)
# check if we are in bounds
if x_coord > self.map.info.width or x_coord < 0:
return float('nan')
if y_coord > self.map.info.height or y_coord < 0:
return float('nan')
ind = x_coord + y_coord*self.map.info.width
if ind >= self.map.info.width*self.map.info.height or ind < 0:
return float('nan')
return ind
def map_calc_range(self,x,y,theta):
''' this is for a beam model... this is pretty damn slow...'''
(x_curr,y_curr) = (x,y)
ind = self.get_map_index(x_curr, y_curr)
while not(math.isnan(ind)):
if self.map.data[ind] > 0:
return math.sqrt((x - x_curr)**2 + (y - y_curr)**2)
x_curr += self.map.info.resolution*0.5*math.cos(theta)
y_curr += self.map.info.resolution*0.5*math.sin(theta)
ind = self.get_map_index(x_curr, y_curr)
if math.isnan(ind):
return float('nan')
else:
return self.map.info.range_max
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 = MyAMCL.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()
def update_particles_with_laser(self, msg):
laser_xy_theta = MyAMCL.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
for i in range(5,len(msg.ranges),10):
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)
ind = self.get_map_index(end_x,end_y)
if math.isnan(ind):
z = self.laser_max_distance
else:
z = self.closest_occ[ind]
pz += self.z_hit * math.exp(-(z * z) / z_hit_denom)
pz += self.z_rand * z_rand_mult
new_prob += pz**3
p.w = new_prob
@staticmethod
def normalize(z):
return math.atan2(math.sin(z), math.cos(z))
@staticmethod
def angle_diff(a, b):
a = MyAMCL.normalize(a)
b = MyAMCL.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):
bins = np.add.accumulate(probabilities)
return values[np.digitize(random_sample(size), bins)]
@staticmethod
def convert_pose_to_xy_and_theta(pose):
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 initialize_particle_cloud(self):
self.particle_cloud_initialized = True
(x,y,theta) = MyAMCL.convert_pose_to_xy_and_theta(self.odom_pose.pose)
for i in range(self.n_particles):
self.particle_cloud.append(Particle(x=x+gauss(0,.25),y=y+gauss(0,.25),theta=theta+gauss(0,.25)))
self.normalize_particles()
def normalize_particles(self):
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="map"),poses=particles_conv))
def scan_received(self, msg):
if not(self.initialized):
return
if not(self.tf_listener.canTransform("base_footprint",msg.header.frame_id,msg.header.stamp)):
return
if not(self.tf_listener.canTransform("base_footprint","odom",msg.header.stamp)):
return
p = PoseStamped(header=Header(stamp=rospy.Time(0),frame_id=msg.header.frame_id))
self.laser_pose = self.tf_listener.transformPose("base_footprint",p)
p = PoseStamped(header=Header(stamp=msg.header.stamp,frame_id="base_footprint"), pose=Pose())
#p = PoseStamped(header=Header(stamp=msg.header.stamp,frame_id="base_footprint"), pose=Pose(position=Point(x=0.0,y=0.0,z=0.0),orientation=Quaternion(x=0.0,y=0.0,z=0.0,w=0.0)))
self.odom_pose = self.tf_listener.transformPose("odom", p)
new_odom_xy_theta = MyAMCL.convert_pose_to_xy_and_theta(self.odom_pose.pose)
if not(self.particle_cloud_initialized):
self.initialize_particle_cloud()
self.update_robot_pose()
self.current_odom_xy_theta = new_odom_xy_theta
self.fix_map_to_odom_transform(msg)
else:
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])
if math.fabs(delta[0]) > self.d_thresh or math.fabs(delta[1]) > self.d_thresh or math.fabs(delta[2]) > self.a_thresh:
self.update_particles_with_odom(msg)
self.update_robot_pose()
self.update_particles_with_laser(msg)
self.resample_particles()
self.update_robot_pose()
self.fix_map_to_odom_transform(msg)
else:
self.fix_map_to_odom_transform(msg, False)
self.publish_particles(msg)
def fix_map_to_odom_transform(self, msg, recompute_odom_to_map=True):
if recompute_odom_to_map:
(translation, rotation) = MyAMCL.convert_pose_inverse_transform(self.robot_pose)
p = PoseStamped(pose=MyAMCL.convert_translation_rotation_to_pose(translation,rotation),header=Header(stamp=msg.header.stamp,frame_id="base_footprint"))
self.odom_to_map = self.tf_listener.transformPose("odom", p)
(translation, rotation) = MyAMCL.convert_pose_inverse_transform(self.odom_to_map.pose)
self.tf_broadcaster.sendTransform(translation, rotation, msg.header.stamp+rospy.Duration(1.0), "odom", "map")
@staticmethod
def convert_translation_rotation_to_pose(translation,rotation):
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]))
@staticmethod
def convert_pose_inverse_transform(pose):
translation = np.zeros((4,1))
translation[0] = -pose.position.x
translation[1] = -pose.position.y
translation[2] = -pose.position.z
translation[3] = 1.0
rotation = (pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w)
euler_angle = euler_from_quaternion(rotation)
rotation = np.transpose(rotation_matrix(euler_angle[2], [0,0,1])) # the angle is a yaw
transformed_translation = rotation.dot(translation)
translation = (transformed_translation[0], transformed_translation[1], transformed_translation[2])
rotation = quaternion_from_matrix(rotation)
return (translation, rotation)
class World:
"""Object recognition and localization related stuff"""
tf_listener = None
objects = []
def __init__(self):
if World.tf_listener == None:
World.tf_listener = TransformListener()
self._lock = threading.Lock()
self.surface = None
self._tf_broadcaster = TransformBroadcaster()
self._im_server = InteractiveMarkerServer("world_objects")
bb_service_name = "find_cluster_bounding_box"
rospy.wait_for_service(bb_service_name)
self._bb_service = rospy.ServiceProxy(bb_service_name, FindClusterBoundingBox)
rospy.Subscriber("interactive_object_recognition_result", GraspableObjectList, self.receieve_object_info)
self._object_action_client = actionlib.SimpleActionClient("object_detection_user_command", UserCommandAction)
self._object_action_client.wait_for_server()
rospy.loginfo("Interactive object detection action " + "server has responded.")
self.clear_all_objects()
# The following is to get the table information
rospy.Subscriber("tabletop_segmentation_markers", Marker, self.receieve_table_marker)
def _reset_objects(self):
"""Function that removes all objects"""
self._lock.acquire()
for i in range(len(World.objects)):
self._im_server.erase(World.objects[i].int_marker.name)
self._im_server.applyChanges()
if self.surface != None:
self._remove_surface()
self._im_server.clear()
self._im_server.applyChanges()
World.objects = []
self._lock.release()
def receieve_table_marker(self, marker):
"""Callback function for markers to determine table"""
if marker.type == Marker.LINE_STRIP:
if len(marker.points) == 6:
rospy.loginfo("Received a TABLE marker.")
xmin = marker.points[0].x
ymin = marker.points[0].y
xmax = marker.points[2].x
ymax = marker.points[2].y
depth = xmax - xmin
width = ymax - ymin
pose = Pose(marker.pose.position, marker.pose.orientation)
pose.position.x = pose.position.x + xmin + depth / 2
pose.position.y = pose.position.y + ymin + width / 2
dimensions = Vector3(depth, width, 0.01)
self.surface = World._get_surface_marker(pose, dimensions)
self._im_server.insert(self.surface, self.marker_feedback_cb)
self._im_server.applyChanges()
def receieve_object_info(self, object_list):
"""Callback function to receive object info"""
self._lock.acquire()
rospy.loginfo("Received recognized object list.")
if len(object_list.graspable_objects) > 0:
for i in range(len(object_list.graspable_objects)):
models = object_list.graspable_objects[i].potential_models
if len(models) > 0:
object_pose = None
best_confidence = 0.0
for j in range(len(models)):
if best_confidence < models[j].confidence:
object_pose = models[j].pose.pose
best_confidence = models[j].confidence
if object_pose != None:
rospy.logwarn("Adding the recognized object " + "with most confident model.")
self._add_new_object(object_pose, Vector3(0.2, 0.2, 0.2), True, object_list.meshes[i])
else:
rospy.logwarn("... this is not a recognition result, " + "it is probably just segmentation.")
cluster = object_list.graspable_objects[i].cluster
bbox = self._bb_service(cluster)
cluster_pose = bbox.pose.pose
if cluster_pose != None:
rospy.loginfo(
"Adding unrecognized object with pose:"
+ World.pose_to_string(cluster_pose)
+ "\n"
+ "In ref frame"
+ str(bbox.pose.header.frame_id)
)
self._add_new_object(cluster_pose, bbox.box_dims, False)
else:
rospy.logwarn("... but the list was empty.")
self._lock.release()
@staticmethod
def get_pose_from_transform(transform):
"""Returns pose for transformation matrix"""
pos = transform[:3, 3].copy()
rot = tf.transformations.quaternion_from_matrix(transform)
return Pose(Point(pos[0], pos[1], pos[2]), Quaternion(rot[0], rot[1], rot[2], rot[3]))
@staticmethod
def get_matrix_from_pose(pose):
"""Returns the transformation matrix for given pose"""
rotation = [pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w]
transformation = tf.transformations.quaternion_matrix(rotation)
position = [pose.position.x, pose.position.y, pose.position.z]
transformation[:3, 3] = position
return transformation
@staticmethod
def get_absolute_pose(arm_state):
"""Returns absolute pose of an end effector state"""
if arm_state.refFrame == ArmState.OBJECT:
arm_state_copy = ArmState(
arm_state.refFrame,
Pose(arm_state.ee_pose.position, arm_state.ee_pose.orientation),
arm_state.joint_pose[:],
arm_state.refFrameObject,
)
World.convert_ref_frame(arm_state_copy, ArmState.ROBOT_BASE)
return arm_state_copy.ee_pose
else:
return arm_state.ee_pose
@staticmethod
def get_most_similar_obj(ref_object, ref_frame_list):
"""Finds the most similar object in the world"""
best_dist = 10000
chosen_obj_index = -1
for i in range(len(ref_frame_list)):
dist = World.object_dissimilarity(ref_frame_list[i], ref_object)
if dist < best_dist:
best_dist = dist
chosen_obj_index = i
if chosen_obj_index == -1:
rospy.logwarn("Did not find a similar object..")
return None
else:
print "Object dissimilarity is --- ", best_dist
if best_dist > 0.075:
rospy.logwarn("Found some objects, but not similar enough.")
return None
else:
rospy.loginfo("Most similar to new object " + str(chosen_obj_index))
return ref_frame_list[chosen_obj_index]
@staticmethod
def _get_mesh_marker(marker, mesh):
"""Function that generated a marker from a mesh"""
marker.type = Marker.TRIANGLE_LIST
index = 0
marker.scale = Vector3(1.0, 1.0, 1.0)
while index + 2 < len(mesh.triangles):
if (
(mesh.triangles[index] < len(mesh.vertices))
and (mesh.triangles[index + 1] < len(mesh.vertices))
and (mesh.triangles[index + 2] < len(mesh.vertices))
):
marker.points.append(mesh.vertices[mesh.triangles[index]])
marker.points.append(mesh.vertices[mesh.triangles[index + 1]])
marker.points.append(mesh.vertices[mesh.triangles[index + 2]])
index += 3
else:
rospy.logerr("Mesh contains invalid triangle!")
break
return marker
def _add_new_object(self, pose, dimensions, is_recognized, mesh=None):
"""Function to add new objects"""
dist_threshold = 0.02
to_remove = None
if is_recognized:
# Temporary HACK for testing.
# Will remove all recognition completely if this works.
return False
# Check if there is already an object
for i in range(len(World.objects)):
distance = World.pose_distance(World.objects[i].object.pose, pose)
if distance < dist_threshold:
if World.objects[i].is_recognized:
rospy.loginfo(
"Previously recognized object at " + "the same location, will not add this object."
)
return False
else:
rospy.loginfo(
"Previously unrecognized object at "
+ "the same location, will replace it with the "
+ "recognized object."
)
to_remove = i
break
if to_remove != None:
self._remove_object(to_remove)
n_objects = len(World.objects)
World.objects.append(WorldObject(pose, n_objects, dimensions, is_recognized))
int_marker = self._get_object_marker(len(World.objects) - 1, mesh)
World.objects[-1].int_marker = int_marker
self._im_server.insert(int_marker, self.marker_feedback_cb)
self._im_server.applyChanges()
World.objects[-1].menu_handler.apply(self._im_server, int_marker.name)
self._im_server.applyChanges()
return True
else:
for i in range(len(World.objects)):
if World.pose_distance(World.objects[i].object.pose, pose) < dist_threshold:
rospy.loginfo("Previously detected object at the same" + "location, will not add this object.")
return False
n_objects = len(World.objects)
World.objects.append(WorldObject(pose, n_objects, dimensions, is_recognized))
int_marker = self._get_object_marker(len(World.objects) - 1)
World.objects[-1].int_marker = int_marker
self._im_server.insert(int_marker, self.marker_feedback_cb)
self._im_server.applyChanges()
World.objects[-1].menu_handler.apply(self._im_server, int_marker.name)
self._im_server.applyChanges()
return True
def _remove_object(self, to_remove):
"""Function to remove object by index"""
obj = World.objects.pop(to_remove)
rospy.loginfo("Removing object " + obj.int_marker.name)
self._im_server.erase(obj.int_marker.name)
self._im_server.applyChanges()
# if (obj.is_recognized):
# for i in range(len(World.objects)):
# if ((World.objects[i].is_recognized)
# and World.objects[i].index>obj.index):
# World.objects[i].decrease_index()
# self.n_recognized -= 1
# else:
# for i in range(len(World.objects)):
# if ((not World.objects[i].is_recognized) and
# World.objects[i].index>obj.index):
# World.objects[i].decrease_index()
# self.n_unrecognized -= 1
def _remove_surface(self):
"""Function to request removing surface"""
rospy.loginfo("Removing surface")
self._im_server.erase("surface")
self._im_server.applyChanges()
self.surface = None
def _get_object_marker(self, index, mesh=None):
"""Generate a marker for world objects"""
int_marker = InteractiveMarker()
int_marker.name = World.objects[index].get_name()
int_marker.header.frame_id = "base_link"
int_marker.pose = World.objects[index].object.pose
int_marker.scale = 1
button_control = InteractiveMarkerControl()
button_control.interaction_mode = InteractiveMarkerControl.BUTTON
button_control.always_visible = True
object_marker = Marker(
type=Marker.CUBE,
id=index,
lifetime=rospy.Duration(2),
scale=World.objects[index].object.dimensions,
header=Header(frame_id="base_link"),
color=ColorRGBA(0.2, 0.8, 0.0, 0.6),
pose=World.objects[index].object.pose,
)
if mesh != None:
object_marker = World._get_mesh_marker(object_marker, mesh)
button_control.markers.append(object_marker)
text_pos = Point()
text_pos.x = World.objects[index].object.pose.position.x
text_pos.y = World.objects[index].object.pose.position.y
text_pos.z = World.objects[index].object.pose.position.z + World.objects[index].object.dimensions.z / 2 + 0.06
button_control.markers.append(
Marker(
type=Marker.TEXT_VIEW_FACING,
id=index,
scale=Vector3(0, 0, 0.03),
text=int_marker.name,
color=ColorRGBA(0.0, 0.0, 0.0, 0.5),
header=Header(frame_id="base_link"),
pose=Pose(text_pos, Quaternion(0, 0, 0, 1)),
)
)
int_marker.controls.append(button_control)
return int_marker
@staticmethod
def _get_surface_marker(pose, dimensions):
""" Function that generates a surface marker"""
int_marker = InteractiveMarker()
int_marker.name = "surface"
int_marker.header.frame_id = "base_link"
int_marker.pose = pose
int_marker.scale = 1
button_control = InteractiveMarkerControl()
button_control.interaction_mode = InteractiveMarkerControl.BUTTON
button_control.always_visible = True
object_marker = Marker(
type=Marker.CUBE,
id=2000,
lifetime=rospy.Duration(2),
scale=dimensions,
header=Header(frame_id="base_link"),
color=ColorRGBA(0.8, 0.0, 0.4, 0.4),
pose=pose,
)
button_control.markers.append(object_marker)
text_pos = Point()
position = pose.position
dimensions = dimensions
text_pos.x = position.x + dimensions.x / 2 - 0.06
text_pos.y = position.y - dimensions.y / 2 + 0.06
text_pos.z = position.z + dimensions.z / 2 + 0.06
text_marker = Marker(
type=Marker.TEXT_VIEW_FACING,
id=2001,
scale=Vector3(0, 0, 0.03),
text=int_marker.name,
color=ColorRGBA(0.0, 0.0, 0.0, 0.5),
header=Header(frame_id="base_link"),
pose=Pose(text_pos, Quaternion(0, 0, 0, 1)),
)
button_control.markers.append(text_marker)
int_marker.controls.append(button_control)
return int_marker
@staticmethod
def get_frame_list():
"""Function that returns the list of ref. frames"""
objects = []
for i in range(len(World.objects)):
objects.append(World.objects[i].object)
return objects
@staticmethod
def has_objects():
"""Function that checks if there are any objects"""
return len(World.objects) > 0
@staticmethod
def object_dissimilarity(obj1, obj2):
"""Distance between two objects"""
dims1 = obj1.dimensions
dims2 = obj2.dimensions
return norm(array([dims1.x, dims1.y, dims1.z]) - array([dims2.x, dims2.y, dims2.z]))
@staticmethod
def get_ref_from_name(ref_name):
"""Ref. frame type from ref. frame name"""
if ref_name == "base_link":
return ArmState.ROBOT_BASE
else:
return ArmState.OBJECT
@staticmethod
def convert_ref_frame(arm_frame, ref_frame, ref_frame_obj=Object()):
"""Transforms an arm frame to a new ref. frame"""
if ref_frame == ArmState.ROBOT_BASE:
if arm_frame.refFrame == ArmState.ROBOT_BASE:
pass
# rospy.logwarn('No reference frame transformations ' +
#'needed (both absolute).')
elif arm_frame.refFrame == ArmState.OBJECT:
abs_ee_pose = World.transform(arm_frame.ee_pose, arm_frame.refFrameObject.name, "base_link")
arm_frame.ee_pose = abs_ee_pose
arm_frame.refFrame = ArmState.ROBOT_BASE
arm_frame.refFrameObject = Object()
else:
rospy.logerr(
"Unhandled reference frame conversion:" + str(arm_frame.refFrame) + " to " + str(ref_frame)
)
elif ref_frame == ArmState.OBJECT:
if arm_frame.refFrame == ArmState.ROBOT_BASE:
rel_ee_pose = World.transform(arm_frame.ee_pose, "base_link", ref_frame_obj.name)
arm_frame.ee_pose = rel_ee_pose
arm_frame.refFrame = ArmState.OBJECT
arm_frame.refFrameObject = ref_frame_obj
elif arm_frame.refFrame == ArmState.OBJECT:
if arm_frame.refFrameObject.name == ref_frame_obj.name:
pass
# rospy.logwarn('No reference frame transformations ' +
#'needed (same object).')
else:
rel_ee_pose = World.transform(arm_frame.ee_pose, arm_frame.refFrameObject.name, ref_frame_obj.name)
arm_frame.ee_pose = rel_ee_pose
arm_frame.refFrame = ArmState.OBJECT
arm_frame.refFrameObject = ref_frame_obj
else:
rospy.logerr(
"Unhandled reference frame conversion:" + str(arm_frame.refFrame) + " to " + str(ref_frame)
)
return arm_frame
@staticmethod
def has_object(object_name):
"""Checks if the world contains the object"""
for obj in World.objects:
if obj.object.name == object_name:
return True
return False
@staticmethod
def is_frame_valid(object_name):
"""Checks if the frame is valid for transforms"""
return (object_name == "base_link") or World.has_object(object_name)
@staticmethod
def transform(pose, from_frame, to_frame):
""" Function to transform a pose between two ref. frames
if there is a TF exception or object does not exist it
will return the pose back without any transforms"""
if World.is_frame_valid(from_frame) and World.is_frame_valid(to_frame):
pose_stamped = PoseStamped()
try:
common_time = World.tf_listener.getLatestCommonTime(from_frame, to_frame)
pose_stamped.header.stamp = common_time
pose_stamped.header.frame_id = from_frame
pose_stamped.pose = pose
rel_ee_pose = World.tf_listener.transformPose(to_frame, pose_stamped)
return rel_ee_pose.pose
except tf.Exception:
rospy.logerr("TF exception during transform.")
return pose
except rospy.ServiceException:
rospy.logerr("Exception during transform.")
return pose
else:
rospy.logwarn("One of the frame objects might not exist: " + from_frame + " or " + to_frame)
return pose
@staticmethod
def pose_to_string(pose):
"""For printing a pose to stdout"""
return (
"Position: "
+ str(pose.position.x)
+ ", "
+ str(pose.position.y)
+ ", "
+ str(pose.position.z)
+ "\n"
+ "Orientation: "
+ str(pose.orientation.x)
+ ", "
+ str(pose.orientation.y)
+ ", "
+ str(pose.orientation.z)
+ ", "
+ str(pose.orientation.w)
+ "\n"
)
def _publish_tf_pose(self, pose, name, parent):
""" Publishes a TF for object pose"""
if pose != None:
pos = (pose.position.x, pose.position.y, pose.position.z)
rot = (pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w)
self._tf_broadcaster.sendTransform(pos, rot, rospy.Time.now(), name, parent)
def update_object_pose(self):
""" Function to externally update an object pose"""
Response.perform_gaze_action(GazeGoal.LOOK_DOWN)
while (
Response.gaze_client.get_state() == GoalStatus.PENDING
or Response.gaze_client.get_state() == GoalStatus.ACTIVE
):
time.sleep(0.1)
if Response.gaze_client.get_state() != GoalStatus.SUCCEEDED:
rospy.logerr("Could not look down to take table snapshot")
return False
rospy.loginfo("Looking at table now.")
goal = UserCommandGoal(UserCommandGoal.RESET, False)
self._object_action_client.send_goal(goal)
while (
self._object_action_client.get_state() == GoalStatus.ACTIVE
or self._object_action_client.get_state() == GoalStatus.PENDING
):
time.sleep(0.1)
rospy.loginfo("Object recognition has been reset.")
rospy.loginfo("STATUS: " + self._object_action_client.get_goal_status_text())
self._reset_objects()
if self._object_action_client.get_state() != GoalStatus.SUCCEEDED:
rospy.logerr("Could not reset recognition.")
return False
# Do segmentation
goal = UserCommandGoal(UserCommandGoal.SEGMENT, False)
self._object_action_client.send_goal(goal)
while (
self._object_action_client.get_state() == GoalStatus.ACTIVE
or self._object_action_client.get_state() == GoalStatus.PENDING
):
time.sleep(0.1)
rospy.loginfo("Table segmentation is complete.")
rospy.loginfo("STATUS: " + self._object_action_client.get_goal_status_text())
if self._object_action_client.get_state() != GoalStatus.SUCCEEDED:
rospy.logerr("Could not segment.")
return False
# Do recognition
goal = UserCommandGoal(UserCommandGoal.RECOGNIZE, False)
self._object_action_client.send_goal(goal)
while (
self._object_action_client.get_state() == GoalStatus.ACTIVE
or self._object_action_client.get_state() == GoalStatus.PENDING
):
time.sleep(0.1)
rospy.loginfo("Objects on the table have been recognized.")
rospy.loginfo("STATUS: " + self._object_action_client.get_goal_status_text())
# Record the result
if self._object_action_client.get_state() == GoalStatus.SUCCEEDED:
wait_time = 0
total_wait_time = 5
while not World.has_objects() and wait_time < total_wait_time:
time.sleep(0.1)
wait_time += 0.1
if not World.has_objects():
rospy.logerr("Timeout waiting for a recognition result.")
return False
else:
rospy.loginfo("Got the object list.")
return True
else:
rospy.logerr("Could not recognize.")
return False
def clear_all_objects(self):
"""Removes all objects from the world"""
goal = UserCommandGoal(UserCommandGoal.RESET, False)
self._object_action_client.send_goal(goal)
while (
self._object_action_client.get_state() == GoalStatus.ACTIVE
or self._object_action_client.get_state() == GoalStatus.PENDING
):
time.sleep(0.1)
rospy.loginfo("Object recognition has been reset.")
rospy.loginfo("STATUS: " + self._object_action_client.get_goal_status_text())
if self._object_action_client.get_state() == GoalStatus.SUCCEEDED:
rospy.loginfo("Successfully reset object localization pipeline.")
self._reset_objects()
self._remove_surface()
def get_nearest_object(self, arm_pose):
"""Gives a pointed to the nearest object"""
dist_threshold = 0.4
def chObj(cur, obj):
dist = World.pose_distance(obj.object.pose, arm_pose)
return (dist, obj.object) if (dist < cur[0]) else cur
return reduce(chObj, World.objects, (dist_threshold, None))[1]
@staticmethod
def pose_distance(pose1, pose2, is_on_table=True):
"""Distance between two world poses"""
if pose1 == [] or pose2 == []:
return 0.0
else:
if is_on_table:
arr1 = array([pose1.position.x, pose1.position.y])
arr2 = array([pose2.position.x, pose2.position.y])
else:
arr1 = array([pose1.position.x, pose1.position.y, pose1.position.z])
arr2 = array([pose2.position.x, pose2.position.y, pose2.position.z])
dist = norm(arr1 - arr2)
if dist < 0.0001:
dist = 0
return dist
def marker_feedback_cb(self, feedback):
"""Callback for when feedback from a marker is received"""
if feedback.event_type == InteractiveMarkerFeedback.BUTTON_CLICK:
rospy.loginfo("Clicked on object " + str(feedback.marker_name))
rospy.loginfo("Number of objects " + str(len(World.objects)))
else:
rospy.loginfo("Unknown event" + str(feedback.event_type))
def update(self):
"""Update function called in a loop"""
# Visualize the detected object
is_world_changed = False
self._lock.acquire()
if World.has_objects():
to_remove = None
for i in range(len(World.objects)):
self._publish_tf_pose(World.objects[i].object.pose, World.objects[i].get_name(), "base_link")
if World.objects[i].is_removed:
to_remove = i
if to_remove != None:
self._remove_object(to_remove)
is_world_changed = True
self._lock.release()
return is_world_changed
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, 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
gettingMap = rospy.ServiceProxy('static_map',GetMap)
myMap = gettingMap().map
# for now we have commented out the occupancy field initialization until you can successfully fetch the map
self.occupancy_field = OccupancyField(myMap)
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()
best_particle = Particle(0,0,0,0)
for particle in self.particle_cloud:
if particle.w > best_particle.w:
best_particle = particle
self.robot_pose = best_particle.as_pose()
#print "updated pose" + str(self.robot_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
for particle in self.particle_cloud:
psi = math.atan2(delta[1],delta[0])- old_odom_xy_theta[2]
r = math.sqrt((delta[0])**2 + (delta[1])**2)
particle.x=particle.x+ r*math.cos(old_odom_xy_theta[2]+psi)
particle.y = particle.y + r*math.sin(old_odom_xy_theta[2]+psi)
particle.theta = old_odom_xy_theta[2] + 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.
"""
# make sure the distribution is normalized
self.normalize_particles()
# TODO: fill out the rest of the implementation
new_particles = []
probabilities = []
for particle in self.particle_cloud:
probabilities.append(particle.w)
new_particles = ParticleFilter.draw_random_sample(self.particle_cloud, probabilities, len(self.particle_cloud))
self.particle_cloud = new_particles
print 'Particle cloud element 0' + '%s' %str(self.particle_cloud[0].x) + str(self.particle_cloud[0].y)
print 'Particle cloud element 1' + '%s' %str(self.particle_cloud[1].x) + str(self.particle_cloud[1].y)
def update_particles_with_laser(self, msg):
""" Updates the particle weights in response to the scan contained in the msg """
# TODO: implement this
#print str(msg.range[0])
#We ctually need to go through all of the range so a for loop form 0 to 360
#We also need to delete any ranges that return 0 because that is a false reading and causes problems
#print msg
for part in self.particle_cloud:
total_distace = 0
average_distance = 0
count = 0
for angle in range(359):
distance_in_front = msg.ranges[angle]
if distance_in_front == 0:
pass
else:
count +=1
rad = angle/360 * 2 * math.pi
part.x = part.x + distance_in_front*math.cos(part.theta + rad)
part.y = part.y + distance_in_front*math.sin(part.theta + rad)
distance = OccupancyField.get_closest_obstacle_distance(self.occupancy_field, part.x, part.y)
total_distace += distance
average_distance = total_distace/count
part.w=(math.e**((-average_distance)**2))
#pass
@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 = []
#one particle at origin
#self.particle_cloud.append(Particle(0,0,0))
# TODO create particles
for i in range(self.n_particles):
self.particle_cloud.append(Particle(randn(), randn(),randn())) #add robot location guess to each number
self.normalize_particles()
self.update_robot_pose()
#print self.particle_cloud
def normalize_particles(self):
""" Make sure the particle weights define a valid distribution (i.e. sum to 1.0) """
pass
# TODO: implement this
#add up all weights of all particles, divide each particle by this number
sumWeight = 0
for particle in self.particle_cloud:
sumWeight += particle.w
for particle in self.particle_cloud:
particle.w /=sumWeight
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 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 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 FlyerViz(object):
"""
The main purpose of this node is to listen to telemetry from the flyer and use it to generate
messages for the various rviz display types (in most cases, Marker or MarkerArray).
"""
# latest_llstatus = None
latest_odom = None
latest_controller_status = None
def __init__(self):
self.init_params()
self.init_publishers()
self.init_vars()
self.init_viz()
self.init_subscribers()
self.init_timers()
rospy.loginfo("Initialization complete")
def init_params(self):
self.publish_freq = rospy.get_param("~publish_freq", 20.0)
self.subscriber_topic_prefix = rospy.get_param("~subscriber_topic_prefix", "downlink/")
def init_publishers(self):
self.mpub = rospy.Publisher("flyer_viz", Marker)
self.mapub = rospy.Publisher("flyer_viz_array", MarkerArray)
self.tfbr = TransformBroadcaster()
def init_vars(self):
self.trajectory = np.empty((1000, 3))
self.n_trajectory = 0
def init_viz(self):
self.mode_t = TextMarker("mode", "", (0, 0, 0))
self.heading_marker = HeadingMarker("heading", (0, 0, 0))
self.vlm = VerticalLineMarker("altitude", (1, 1), color=Colors.WHITE + Alpha(0.5))
self.alt_t = TextMarker("altitude", "0.0", (0, 0, 0))
self.pos_t = TextMarker("position", "0.0,0.0", (0, 0, 0))
self.trail = TrailMarker("trail", [], colors=[], color=Colors.BLUE + Alpha(0.8))
self.trail.set_max_points(500)
self.ground_trail = TrailMarker("ground_track", [], color=Colors.WHITE + Alpha(0.2))
self.ground_trail.set_max_points(500)
self.boundary = PolygonMarker(
"boundary",
((1.5, 1.5, 0), (-1.5, 1.5, 0), (-1.5, -1.5, 0), (1.5, -1.5, 0)),
color=Colors.RED + Alpha(0.5),
width=0.02,
)
self.mg = MarkerGroup(self.mapub)
self.mg.add(
self.mode_t,
self.vlm,
self.alt_t,
self.pos_t,
self.trail,
self.ground_trail,
self.heading_marker,
self.boundary,
)
def init_subscribers(self):
prefix = self.subscriber_topic_prefix
# self.llstatus_sub = rospy.Subscriber(prefix+'autopilot/LL_STATUS', LLStatus, self.llstatus_callback) # AscTec specific.. how to make more generic?
self.odom_sub = rospy.Subscriber(prefix + "estimator/output", Odometry, self.odom_callback)
self.controller_status_sub = rospy.Subscriber(
prefix + "controller/status", controller_status, self.controller_status_callback
)
def init_timers(self):
self.publish_timer = Timer(rospy.Duration(1 / self.publish_freq), self.publish_timer_callback)
def publish_timer_callback(self, event):
now = rospy.Time.now()
if self.latest_controller_status is not None:
self.mode_t.set(text=self.latest_controller_status.active_mode)
if self.latest_odom is not None:
pos = self.latest_odom.pose.pose.position
loppo = self.latest_odom.pose.pose.orientation
ori_ypr = tft.euler_from_quaternion((loppo.x, loppo.y, loppo.z, loppo.w), "rzyx")
self.vlm.set((pos.x, pos.y), zend=pos.z)
self.mode_t.set(pos=(pos.x, pos.y, pos.z - 0.1))
self.alt_t.set(text="%.3f" % -pos.z, pos=(pos.x, pos.y, pos.z / 2))
self.pos_t.set(text="%.2f, %.2f" % (pos.x, pos.y), pos=(pos.x, pos.y, 0.02))
self.heading_marker.set(pos=(pos.x, pos.y, pos.z), heading=degrees(ori_ypr[0]))
self.tfbr.sendTransform(
(pos.x, pos.y, pos.z), (loppo.x, loppo.y, loppo.z, loppo.w), now, "/viz/flyer_axes", "/ned"
)
self.tfbr.sendTransform((pos.y, pos.x, -pos.z), (0, 0, 0, 1), now, "/viz/chase", "/enu")
self.tfbr.sendTransform(
(0, 0, 0), tft.quaternion_from_euler(0, radians(180), 0, "rzyx"), now, "/viz/onboard", "/viz/flyer_axes"
)
self.mg.publish(now)
# def llstatus_callback(self, msg):
# self.latest_llstatus = msg
def controller_status_callback(self, msg):
self.latest_controller_status = msg
def odom_callback(self, msg):
self.latest_odom = msg
pos = self.latest_odom.pose.pose.position
self.trajectory[self.n_trajectory, :] = (pos.x, pos.y, pos.z)
self.n_trajectory += 1
if self.n_trajectory == self.trajectory.shape[0]:
self.trajectory.resize((self.n_trajectory + 1000, 3))
self.trail.append_points([(pos.x, pos.y, pos.z)], [alt_color(-pos.z)])
self.ground_trail.append_points([(pos.x, pos.y, 0)])
mindist = min([a - b for a, b in ((XMAX, pos.x), (pos.x, XMIN), (YMAX, pos.y), (pos.y, YMIN))])
if mindist <= 0:
bcolor = Colors.RED + Alpha(1.0)
elif mindist <= WARN_DIST:
bcolor = Colors.YELLOW + Alpha(1.0)
else:
bcolor = Colors.GREEN + Alpha(1.0)
self.boundary.set(color=bcolor)
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 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 TransformLearner(object):
def __init__(self):
rospy.init_node("learn_transform")
self.tf_listener = TransformListener()
self.tf_broadcaster = TransformBroadcaster()
self.possible_base_link_poses = []
self.baselink_averages = []
self.rate = rospy.Rate(20)
self.markers = { "/PATTERN_1": Pose( [-0.56297, 0.11, 0.0035],
[0.0, 0.0, 0.0, 1.0] ),
"/PATTERN_2": Pose( [-0.45097, 0.11, 0.0035],
[0.0, 0.0, 0.0, 1.0] ),
"/PATTERN_3": Pose( [-0.34097, 0.0, 0.0035],
[0.0, 0.0, 0.0, 1.0] ),
"/PATTERN_4": Pose( [-0.34097, -0.11, 0.0035],
[0.0, 0.0, 0.0, 1.0] ),
"/PATTERN_5": Pose( [-0.45097, -0.11, 0.0035],
[0.0, 0.0, 0.0, 1.0] ),
"/PATTERN_6": Pose( [-0.56297, 0.0, 0.0035],
[0.0, 0.0, 0.0, 1.0] )
}
self.run()
def run(self):
calibration_complete = False
while ( not rospy.is_shutdown()) and (not calibration_complete):
for index, marker in enumerate( self.markers.items() ):
pose_tmp = self.publish_and_get_pose( marker[0], marker[1], index )
if pose_tmp != None:
self.possible_base_link_poses.append( pose_tmp )
if len( self.possible_base_link_poses ) != 0:
best_guess_base_link_to_camera = self.average_poses()
self.baselink_averages.append( best_guess_base_link_to_camera )
calibration_complete = self.check_end_of_calibration()
#print best_guess_base_link_to_camera
print str(best_guess_base_link_to_camera.position.x) + " " + str(best_guess_base_link_to_camera.position.y) + " " +str(best_guess_base_link_to_camera.position.z) + " " + str(best_guess_base_link_to_camera.orientation.x) + " " + str(best_guess_base_link_to_camera.orientation.y) + " " + str(best_guess_base_link_to_camera.orientation.z) + " " + str(best_guess_base_link_to_camera.orientation.w)
self.tf_broadcaster.sendTransform( (best_guess_base_link_to_camera.position.x, best_guess_base_link_to_camera.position.y,
best_guess_base_link_to_camera.position.z),
(best_guess_base_link_to_camera.orientation.x, best_guess_base_link_to_camera.orientation.y,
best_guess_base_link_to_camera.orientation.z, best_guess_base_link_to_camera.orientation.w),
rospy.Time.now(),
"/camera_link",
"/computed_base_link" )
self.rate.sleep()
def average_poses(self):
"""
Average the list of poses for the different guesses of where the kinect
is compared to the base_link
"""
averaged_pose = Pose()
for pose in self.possible_base_link_poses:
averaged_pose.position.x += pose.position.x
averaged_pose.position.y += pose.position.y
averaged_pose.position.z += pose.position.z
averaged_pose.orientation.x += pose.orientation.x
averaged_pose.orientation.y += pose.orientation.y
averaged_pose.orientation.z += pose.orientation.z
averaged_pose.orientation.w += pose.orientation.w
size = len( self.possible_base_link_poses )
averaged_pose.position.x /= size
averaged_pose.position.y /= size
averaged_pose.position.z /= size
averaged_pose.orientation.x /= size
averaged_pose.orientation.y /= size
averaged_pose.orientation.z /= size
averaged_pose.orientation.w /= size
return averaged_pose
def publish_and_get_pose(self, marker_name, transform_marker_to_base_link, index):
"""
Publishes a transform between the given marker and the base link, then
listen for the transform from /base_link to /camera_link.
The transform between the marker and the base link is known.
"""
trans = None
rot = None
#try to get the pose of the given link_name
# in the base_link frame.
success = False
base_link_name = "/base_link_" + str(index)
for i in range (0, 500):
self.tf_broadcaster.sendTransform( transform_marker_to_base_link.position ,
transform_marker_to_base_link.orientation,
rospy.Time.now(),
base_link_name,
marker_name )
try:
(trans, rot) = self.tf_listener.lookupTransform( base_link_name, '/camera_link',
rospy.Time(0) )
success = True
break
except:
continue
pose = None
if success == False:
return pose
pose = Pose()
pose.position.x = trans[0]
pose.position.y = trans[1]
pose.position.z = trans[2]
pose.orientation.x = rot[0]
pose.orientation.y = rot[1]
pose.orientation.z = rot[2]
pose.orientation.w = rot[3]
return pose
def check_end_of_calibration(self):
#Simple version: difference between the two last averages
if len(self.baselink_averages) > 1:
if ((math.sqrt((self.baselink_averages[len(self.baselink_averages) - 1].position.x - self.baselink_averages[len(self.baselink_averages) - 2].position.x)
* (self.baselink_averages[len(self.baselink_averages) - 1].position.x - self.baselink_averages[len(self.baselink_averages) - 2].position.x)) < CONVERGE_THRESHOLD) and
(math.sqrt((self.baselink_averages[len(self.baselink_averages) - 1].position.y - self.baselink_averages[len(self.baselink_averages) - 2].position.y)
* (self.baselink_averages[len(self.baselink_averages) - 1].position.y - self.baselink_averages[len(self.baselink_averages) - 2].position.y)) < CONVERGE_THRESHOLD) and
(math.sqrt((self.baselink_averages[len(self.baselink_averages) - 1].position.z - self.baselink_averages[len(self.baselink_averages) - 2].position.z)
* (self.baselink_averages[len(self.baselink_averages) - 1].position.z - self.baselink_averages[len(self.baselink_averages) - 2].position.z)) < CONVERGE_THRESHOLD) and
(math.sqrt((self.baselink_averages[len(self.baselink_averages) - 1].orientation.x - self.baselink_averages[len(self.baselink_averages) - 2].orientation.x)
* (self.baselink_averages[len(self.baselink_averages) - 1].orientation.x - self.baselink_averages[len(self.baselink_averages) - 2].orientation.x)) < CONVERGE_THRESHOLD) and
(math.sqrt((self.baselink_averages[len(self.baselink_averages) - 1].orientation.y - self.baselink_averages[len(self.baselink_averages) - 2].orientation.y)
* (self.baselink_averages[len(self.baselink_averages) - 1].orientation.y - self.baselink_averages[len(self.baselink_averages) - 2].orientation.y)) < CONVERGE_THRESHOLD) and
(math.sqrt((self.baselink_averages[len(self.baselink_averages) - 1].orientation.z - self.baselink_averages[len(self.baselink_averages) - 2].orientation.z)
* (self.baselink_averages[len(self.baselink_averages) - 1].orientation.z - self.baselink_averages[len(self.baselink_averages) - 2].orientation.z)) < CONVERGE_THRESHOLD) and
(math.sqrt((self.baselink_averages[len(self.baselink_averages) - 1].orientation.w - self.baselink_averages[len(self.baselink_averages) - 2].orientation.w)
* (self.baselink_averages[len(self.baselink_averages) - 1].orientation.w - self.baselink_averages[len(self.baselink_averages) - 2].orientation.w)) < CONVERGE_THRESHOLD)):
print "This is the calibrated kinect transform value:"
return True
else:
return False
else:
return False
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 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 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 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
robot_pose: estimated position of the robot of type geometry_msgs/Pose
"""
# some constants! :) -emily and franz
TAU = math.pi * 2.0
# to be used in update_particles_with_odom
RADIAL_SIGMA = .03 # meters
ORIENTATION_SIGMA = 0.03 * TAU
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 = 30 # 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
#set self.visualize_weights to True if you want to see a plot of xpos vs weights every time the particles are updated
self.visualize_weights = True
# 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.rawcloud_pub = rospy.Publisher("rawcloud", PoseArray, queue_size=1)
self.odomcloud_pub = rospy.Publisher("odomcloud", PoseArray, queue_size=1)
self.lasercloud_pub = rospy.Publisher("lasercloud", PoseArray, queue_size=1)
self.resamplecloud_pub = rospy.Publisher("resamplecloud", PoseArray, queue_size=1)
self.finalcloud_pub = rospy.Publisher("finalcloud", PoseArray, queue_size=1)
# 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
get_static_map = rospy.ServiceProxy('static_map', GetMap)
self.occupancy_field = OccupancyField(get_static_map().map)
self.robot_pose = Pose()
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()
# compute mean pose by calculating the weighted average of each position and angle
mean_x = 0
mean_y = 0
mean_theta = 0
for particle in self.particle_cloud:
mean_x += particle.w * particle.x
mean_y += particle.w * particle.y
mean_theta += particle.w * particle.theta
mean_particle = Particle(mean_x, mean_y, mean_theta)
self.robot_pose = mean_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)
# 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
r1 = math.atan2(delta[1], delta[0]) - old_odom_xy_theta[2]
delta_distance = np.linalg.norm([delta[0], delta[1]])
r2 = delta[2] - r1
for particle in self.particle_cloud:
# randomly pick the deltas for radial distance, mean angle, and orientation angle
delta_random_radius = np.random.normal(0, ParticleFilter.RADIAL_SIGMA)
delta_random_mean_angle = random_sample() * ParticleFilter.TAU / 2.0
delta_random_orient_angle = np.random.normal(0, ParticleFilter.ORIENTATION_SIGMA)
# calculate the deltas
delta_random_x = delta_random_radius * math.cos(delta_random_mean_angle)
delta_random_y = delta_random_radius * math.sin(delta_random_mean_angle)
# update the mean (add deltas)
particle.theta += r1
particle.x += math.cos(particle.theta) * delta_distance + delta_random_x
particle.y += math.sin(particle.theta) * delta_distance + delta_random_y
particle.theta += r2 + delta_random_orient_angle
# 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()
probabilities = [particle.w for particle in self.particle_cloud]
new_particle_cloud = []
for i in range(self.n_particles):
random_particle = deepcopy(np.random.choice(self.particle_cloud, p=probabilities))
new_particle_cloud.append(random_particle)
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 """
# compare the distance to the closest occupied location
# of the hypothesis and laser scan measurement
# give it a weight inversely proportional to the error
valid_ranges = self.filter_laser(msg.ranges)
for particle in self.particle_cloud:
total_probability_density = 1
for angle in valid_ranges:
radius = valid_ranges[angle]
angle = (angle+.25*ParticleFilter.TAU) % ParticleFilter.TAU
x = math.cos(angle+particle.theta) * radius + particle.x
y = math.sin(angle+particle.theta) * radius + particle.y
dist_to_nearest_neighbor = self.occupancy_field.get_closest_obstacle_distance(x, y)
# calculate probability of nearest neighbor's distance
probability_density = normal(dist_to_nearest_neighbor, .05)
total_probability_density *= 1 + probability_density #the 1+ is hacky
# TODO: make the total_probability_density function more legit
particle.w = total_probability_density
# rospy.loginfo(particle.w)
def visualize_p_weights(self):
""" Produces a plot of particle weights vs. x position """
# close any figures that are open
plt.close('all')
# initialize the things
xpos = np.zeros(len(self.particle_cloud))
weights = np.zeros(len(self.particle_cloud))
x_i = 0
weights_i = 0
# grab the current values
for p in self.particle_cloud:
xpos[x_i] = p.x
weights[weights_i] = p.w
x_i += 1
weights_i += 1
# plotting current xpos and weights
fig = plt.figure()
plt.xlabel('xpos')
plt.ylabel('weights')
plt.title('xpos vs weights')
plt.plot(xpos, weights, 'ro')
plt.show(block=False)
@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):
""" Initialize the particle cloud.
Arguments
"""
rospy.loginfo("initialize particle cloud")
self.particle_cloud = []
map_info = self.occupancy_field.map.info
for i in range(self.n_particles):
x = random_sample()* map_info.width * map_info.resolution * 0.1
if random_sample() > 0.5:
x = -x
y = random_sample()* map_info.height * map_info.resolution * 0.1
if random_sample() > 0.5:
y = -y
theta = random_sample() * math.pi*2
self.particle_cloud.append(Particle(x, y, 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) """
sum = 0
for particle in self.particle_cloud:
sum += particle.w
for particle in self.particle_cloud:
particle.w /= sum
def publish_particles(self, pub):
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
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, 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
rospy.logwarn("can't transform to laser scan")
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
rospy.logwarn("can't transform to base frame")
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))
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.publish_particles(self.rawcloud_pub)
self.update_particles_with_odom(msg) # update based on odometry
self.publish_particles(self.odomcloud_pub)
self.update_particles_with_laser(msg) # update based on laser scan
self.publish_particles(self.lasercloud_pub)
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
if self.visualize_weights:
self.visualize_p_weights()
# publish particles (so things like rviz can see them)
self.publish_particles(self.finalcloud_pub)
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=rospy.Time(0), 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 filter_laser(self, ranges):
""" Takes the message from a laser scan as an array and returns a dictionary of angle:distance pairs"""
valid_ranges = {}
for i in range(len(ranges)):
if ranges[i] > 0.0 and ranges[i] < 3.5:
valid_ranges[i] = ranges[i]
return valid_ranges
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)
number_of_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.number_of_particles = 1000 # 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
# 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 = []
# Fetch map using OccupancyField
rospy.wait_for_service('static_map')
static_map = rospy.ServiceProxy('static_map', GetMap)
self.occupancy_field = OccupancyField(static_map().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()
avg_x = 0
avg_y = 0
theta_x = 0
theta_y = 0
# Multiple x and y by particle weights to find new robot pose
for particle in self.particle_cloud:
avg_x += particle.x * particle.w
avg_y += particle.y * particle.w
theta_x += math.cos(particle.theta) * particle.w
theta_y += math.sin(particle.theta) * particle.w
# Calculate theta using arc tan of x and y components of all thetas multiplied by particle weights
avg_theta = math.atan2(theta_y, theta_x)
avg_particle = Particle(x=avg_x, y=avg_y, theta=avg_theta)
# Update robot pose based on average particle
self.robot_pose = avg_particle.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
temp = []
# Use trigonometry to update particles based on new odometry pose
for particle in self.particle_cloud:
psy = math.atan2(delta[1],delta[0])-old_odom_xy_theta[2]
intermediate_theta = particle.theta + psy
# Calculate radius based on change in x and y
r = math.sqrt(delta[0]**2 + delta[1]**2)
# Update x and y based on radius and new angle
new_x = particle.x + r*math.cos(intermediate_theta) + np.random.randn()*0.1
new_y = particle.y + r*math.sin(intermediate_theta) + np.random.randn()*0.1
# Add change in angle to old angle
new_theta = delta[2]+particle.theta + np.random.randn()*0.1
temp.append(Particle(new_x,new_y,new_theta))
self.particle_cloud = temp
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.
"""
probabilities = []
# create list of particle weights to pass into draw_random_sample for resampling
for particle in self.particle_cloud:
probabilities.append(particle.w)
print particle.w
print '\n'
temp_particle_cloud = self.draw_random_sample(self.particle_cloud, probabilities, 100)
self.particle_cloud = []
for particle in temp_particle_cloud:
for i in range(10):
self.particle_cloud.append(deepcopy(particle))
self.normalize_particles()
def update_particles_with_laser(self, msg):
""" Updates the particle weights in response to the scan contained in the msg """
temp = 0
ranges = []
min_range = 5
for item in msg.ranges:
# set ranges to 5 if the laser scan is 0
if item == 0:
ranges.append(5)
else:
ranges.append(item)
# do weighted averages for cleaner data
for i in range(355):
avg = sum(ranges[i:i+5]) / len(ranges[i:i+5])
if avg < min_range:
min_range = avg
min_theta = (i + 2.5)*math.pi / 180.0
# find the minimum range across 360 angles, this probably caused an issue
r = min_range
# Update particle x, y, theta based on min range, previous particles
for particle in self.particle_cloud:
x = particle.x+r*math.cos(particle.theta + min_theta)
y = particle.y+r*math.sin(particle.theta + min_theta)
temp = self.occupancy_field.get_closest_obstacle_distance(x,y)
# Update particle weights using a sharp Gaussian
particle.w = np.exp(-np.power(temp, 2.) / (2 * np.power(0.3, 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 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 = []
self.particle_cloud.append(Particle(xy_theta[0],xy_theta[1],xy_theta[2]))
# Initialize particle cloud with a decent amount of noise
for i in range (0,self.number_of_particles):
self.particle_cloud.append(Particle(xy_theta[0]+np.random.randn()*.5,xy_theta[1]+np.random.randn()*.5,xy_theta[2]+np.random.randn()*.5))
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) """
particle_sum = 0
# Sum up particle weights to divide by for normalization
for particle in self.particle_cloud:
particle_sum += particle.w
# Make all particle weights add to 1
for particle in self.particle_cloud:
particle.w /= particle_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
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 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 ArmUtils():
wipe_started = False
standoff = 0.368 #0.2 + 0.168 (dist from wrist to fingertips)
torso_min = 0.0115
torso_max = 0.295
dist = 0.
move_arm_error_dict = {
-1 : "PLANNING_FAILED: Could not plan a clear path to goal.",
0 : "Move Arm Action Aborted on Internal Failure.",
1 : "SUCCEEDED",
-2 : "TIMED_OUT",
-3 : "START_STATE_IN_COLLISION: Try freeing the arms manually.",
-4 : "START_STATE_VIOLATES_PATH_CONSTRAINTS",
-5 : "GOAL_IN_COLLISION",
-6 : "GOAL_VIOLATES_PATH_CONSTRAINTS",
-7 : "INVALID_ROBOT_STATE",
-8 : "INCOMPLETE_ROBOT_STATE",
-9 : "INVALID_PLANNER_ID",
-10 : "INVALID_NUM_PLANNING_ATTEMPTS",
-11 : "INVALID_ALLOWED_PLANNING_TIME",
-12 : "INVALID_GROUP_NAME",
-13 : "INVALID_GOAL_JOINT_CONSTRAINTS",
-14 : "INVALID_GOAL_POSITION_CONSTRAINTS",
-15 : "INVALID_GOAL_ORIENTATION_CONSTRAINTS",
-16 : "INVALID_PATH_JOINT_CONSTRAINTS",
-17 : "INVALID_PATH_POSITION_CONSTRAINTS",
-18 : "INVALID_PATH_ORIENTATION_CONSTRAINTS",
-19 : "INVALID_TRAJECTORY",
-20 : "INVALID_INDEX",
-21 : "JOINT_LIMITS_VIOLATED",
-22 : "PATH_CONSTRAINTS_VIOLATED",
-23 : "COLLISION_CONSTRAINTS_VIOLATED",
-24 : "GOAL_CONSTRAINTS_VIOLATED",
-25 : "JOINTS_NOT_MOVING",
-26 : "TRAJECTORY_CONTROLLER_FAILED",
-27 : "FRAME_TRANSFORM_FAILURE",
-28 : "COLLISION_CHECKING_UNAVAILABLE",
-29 : "ROBOT_STATE_STALE",
-30 : "SENSOR_INFO_STALE",
-31 : "NO_IK_SOLUTION: Cannot reach goal configuration.",
-32 : "INVALID_LINK_NAME",
-33 : "IK_LINK_IN_COLLISION: Cannot reach goal configuration without contact.",
-34 : "NO_FK_SOLUTION",
-35 : "KINEMATICS_STATE_IN_COLLISION",
-36 : "INVALID_TIMEOUT"
}
def __init__(self, tfListener=None):
self.move_left_arm_client = actionlib.SimpleActionClient('move_left_arm', arm_navigation_msgs.msg.MoveArmAction)
rospy.loginfo("Waiting for move_left_arm server")
if self.move_left_arm_client.wait_for_server():
rospy.loginfo("Found move_left_arm server")
else:
rospy.logwarn("Cannot find move_left_arm server")
self.l_arm_traj_client = actionlib.SimpleActionClient('l_arm_controller/joint_trajectory_action', JointTrajectoryAction)
rospy.loginfo("Waiting for l_arm_controller/joint_trajectory_action server")
if self.l_arm_traj_client.wait_for_server(rospy.Duration(5)):
rospy.loginfo("Found l_arm_controller/joint_trajectory_action server")
else:
rospy.logwarn("Cannot find l_arm_controller/joint_trajectory_action server")
self.l_arm_follow_traj_client = actionlib.SimpleActionClient('l_arm_controller/follow_joint_trajectory', FollowJointTrajectoryAction)
rospy.loginfo("Waiting for l_arm_controller/follow_joint_trajectory server")
if self.l_arm_follow_traj_client.wait_for_server(rospy.Duration(5)):
rospy.loginfo("Found l_arm_controller/follow_joint_trajectory server")
else:
rospy.logwarn("Cannot find l_arm_controller/follow_joint_trajectory server")
self.torso_client = actionlib.SimpleActionClient('torso_controller/position_joint_action', SingleJointPositionAction)
rospy.loginfo("Waiting for torso_controller/position_joint_action server")
if self.torso_client.wait_for_server(rospy.Duration(5)):
rospy.loginfo("Found torso_controller/position_joint_action server")
else:
rospy.logwarn("Cannot find torso_controller/position_joint_action server")
self.l_gripper_client = actionlib.SimpleActionClient('l_gripper_controller/gripper_action', Pr2GripperCommandAction)
rospy.loginfo("Waiting for l_gripper_controller/gripper_action server")
if self.l_gripper_client.wait_for_server(rospy.Duration(5)):
rospy.loginfo("Found l_gripper_controller/gripper_action server")
else:
rospy.logwarn("Cannot find l_gripper_controller/gripper_action server")
rospy.loginfo("Waiting for l_utility_frame_service")
try:
rospy.wait_for_service('/l_utility_frame_update', 7.0)
self.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")
#self.joint_state_lock = RLock()
rospy.Subscriber('l_arm_controller/state', JointTrajectoryControllerState, self.update_joint_state)
self.torso_state_lock = RLock()
rospy.Subscriber('torso_controller/state', JointTrajectoryControllerState, self.update_torso_state)
#self.l_arm_command = rospy.Publisher('/l_arm_controller/command', JointTrajectory )
self.wt_log_out = rospy.Publisher('wt_log_out', String)
if tfListener is None:
self.tf = TransformListener()
else:
self.tf = tfListener
self.tfb = TransformBroadcaster()
rospy.loginfo("Waiting for FK/IK Solver services")
try:
#rospy.wait_for_service('/pr2_left_arm_kinematics/get_fk')
#rospy.wait_for_service('/pr2_left_arm_kinematics/get_fk_solver_info')
rospy.wait_for_service('/pr2_left_arm_kinematics/get_ik')
rospy.wait_for_service('/pr2_left_arm_kinematics/get_ik_solver_info')
#self.fk_info_proxy = rospy.ServiceProxy('/pr2_left_arm_kinematics/get_fk_solver_info',
# GetKinematicSolverInfo)
#self.fk_info = self.fk_info_proxy()
#self.fk_pose_proxy = rospy.ServiceProxy('/pr2_left_arm_kinematics/get_fk', GetPositionFK, True)
self.ik_info_proxy = rospy.ServiceProxy('/pr2_left_arm_kinematics/get_ik_solver_info',
GetKinematicSolverInfo)
self.ik_info = self.ik_info_proxy()
self.ik_pose_proxy = rospy.ServiceProxy('/pr2_left_arm_kinematics/get_ik', GetPositionIK, True)
rospy.loginfo("Found FK/IK Solver services")
except:
rospy.logerr("Could not find FK/IK Solver services")
self.set_planning_scene_diff_name = '/environment_server/set_planning_scene_diff'
rospy.wait_for_service('/environment_server/set_planning_scene_diff')
self.set_planning_scene_diff = rospy.ServiceProxy('/environment_server/set_planning_scene_diff', SetPlanningSceneDiff)
self.set_planning_scene_diff(SetPlanningSceneDiffRequest())
def update_joint_state(self, jtcs):
#self.joint_state_lock.acquire()
self.joint_state_time = jtcs.header.stamp
self.joint_state_act = jtcs.actual
self.joint_state_des = jtcs.desired
self.joint_state_err = jtcs.error
#self.joint_state_lock.release()
def update_torso_state(self, jtcs):
self.torso_state_lock.acquire()
self.torso_state = jtcs
self.torso_state_lock.release()
def curr_pose(self):
print "getting current pose"
(trans,rot) = self.tf.lookupTransform('/torso_lift_link', '/l_wrist_roll_link', rospy.Time(0))
cp = PoseStamped()
cp.header.stamp = rospy.Time.now()
cp.header.frame_id = '/torso_lift_link'
cp.pose.position = Point(*trans)
cp.pose.orientation = Quaternion(*rot)
print cp
return cp
def wait_for_stop(self, wait_time=1, timeout=60):
rospy.sleep(wait_time)
end_time = rospy.Time.now()+rospy.Duration(timeout)
while not self.is_stopped():
if rospy.Time.now()<end_time:
rospy.sleep(wait_time)
else:
raise
def is_stopped(self):
#self.joint_state_lock.acquire()
time = self.joint_state_time
vels = self.joint_state_act.velocities
#self.joint_state_lock.release()
if np.allclose(vels, np.zeros(7)) and (rospy.Time.now()-time)<rospy.Duration(0.1):
return True
else:
return False
# def get_fk(self, msg, frame='/torso_lift_link', link=-1): # get FK pose of a list of joint angles for the arm
# fk_request = GetPositionFKRequest()
# fk_request.header.frame_id = frame
# #fk_request.header.stamp = rospy.Time.now()
# fk_request.fk_link_names = self.fk_info.kinematic_solver_info.link_names
# fk_request.robot_state.joint_state.position = msg #self.joint_state_act.positions
# fk_request.robot_state.joint_state.name = self.fk_info.kinematic_solver_info.joint_names
# try:
# return self.fk_pose_proxy(fk_request).pose_stamped[link]
# except rospy.ServiceException, e:
# rospy.loginfo("FK service did not process request: %s" %str(e))
def adjust_elbow(self, f32):
request = self.form_ik_request(self.curr_pose())
angs = list(request.ik_request.ik_seed_state.joint_state.position)
if f32.data == 1:
angs[2] += 0.25
else:
angs[2] -= 0.25
request.ik_request.ik_seed_state.joint_state.position = angs
ik_goal = self.ik_pose_proxy(request)
if ik_goal.error_code.val == 1:
self.send_joint_angles(ik_goal.solution.joint_state.position)
else:
rospy.loginfo("Cannot adjust elbow position further")
self.wt_log_out.publish(data="Cannot adjust elbow position further")
def gripper(self, position, effort=30):
pos = np.clip(position,0.0, 0.09)
goal = Pr2GripperCommandGoal()
goal.command.position = pos
goal.command.max_effort = effort
self.l_gripper_client.send_goal(goal)
finished_within_time = self.l_gripper_client.wait_for_result(rospy.Duration(15))
if not (finished_within_time):
self.l_gripper_client.cancel_goal()
rospy.loginfo("Timed out moving left gripper")
return False
else:
state = self.l_gripper_client.get_state()
result = self.l_gripper_client.get_result()
if (state == 3): #3 == SUCCEEDED
rospy.loginfo("Gripper Action Succeeded")
return True
else:
rospy.loginfo("Gripper Action Failed")
rospy.loginfo("Failure Result: %s" %result)
return False
def find_approach(self, msg, stdoff=0.15):
stdoff += 0.217#+0.09 #adjust fingertip standoffs for distance to wrist link
self.pose_in = msg
#self.tf.waitForTransform('lh_utility_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(),
"lh_utility_frame",
self.pose_in.header.frame_id)
self.tf.waitForTransform('lh_utility_frame','l_wrist_roll_link', rospy.Time.now(), rospy.Duration(3.0))
goal = PoseStamped()
goal.header.frame_id = 'lh_utility_frame'
goal.header.stamp = rospy.Time.now()
goal.pose.position.z = stdoff
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)
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)
appr.pose.position.z -= 0.01
appr.pose.position.y += 0.0085
return appr
def hand_frame_move(self, x, y=0, z=0, pose = None):
#print "Left Hand Frame Move"
if pose is None:
pose = self.curr_pose()
self.tf.waitForTransform(pose.header.frame_id, '/l_wrist_roll_link', pose.header.stamp, rospy.Duration(3.0))
newpose = self.tf.transformPose('/l_wrist_roll_link', pose)
newpose.pose.position.x += x
newpose.pose.position.y += y
newpose.pose.position.z += z
self.dist = math.sqrt(x**2+y**2+z**2)
return self.tf.transformPose(pose.header.frame_id, newpose)
def pose_frame_move(self, pose, x, y=0, z=0):
self.update_frame(pose)
pose.header.stamp = rospy.Time.now()
self.tf.waitForTransform(pose.header.frame_id, 'lh_utility_frame', pose.header.stamp, rospy.Duration(3.0))
framepose = self.tf.transformPose('lh_utility_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('lh_utility_frame', pose.header.frame_id, pose.header.stamp, rospy.Duration(3.0))
return self.tf.transformPose(pose.header.frame_id, framepose)
def send_angles_wrap(self, angles_in):
cur = list(self.joint_state_act.positions)
diff = np.subtract(angles_in, cur)
risks = np.nonzero(diff>math.pi)
angles_in[risks] -= 2*math.pi
if angles_in[risks] < -4.095:
angles_in[risks] += 4*math.pi
if angles_in[risks] >3.821:
angles_in[risks] -= 2*math.pi
print "Adjusting joint angles!"
print "Current: ", cur
print "Sending: ", angles_in
raw_input('Confirm Joint Angles')
self.send_joint_angles(angles_in)
def build_trajectory(self, finish, start=None, ik_space = 0.015, duration = None, tot_points=None, jagged=False):
if start == None: # if given one pose, use current position as start, else, assume (start, finish)
start = self.curr_pose()
# Based upon distance to travel, determine the number of intermediate poses required
# find linear increments of position.
dist = self.calc_dist(start,finish) #Total distance to travel
ik_steps = math.ceil(dist/ik_space)
print "IK Steps: %s" %ik_steps
ik_fracs = np.linspace(0, 1, ik_steps) #A list of fractional positions along course to evaluate ik
x_gap = finish.pose.position.x - start.pose.position.x
y_gap = finish.pose.position.y - start.pose.position.y
z_gap = finish.pose.position.z - start.pose.position.z
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 each step between start and finish, find a complete pose (linear position changes, and quaternion slerp)
poses = [PoseStamped() for i in range(len(ik_fracs))]
for i,frac in enumerate(ik_fracs):
poses[i].header.stamp = rospy.Time.now()
poses[i].header.frame_id = start.header.frame_id
poses[i].pose.position.x = start.pose.position.x + x_gap*frac
poses[i].pose.position.y = start.pose.position.y + y_gap*frac
poses[i].pose.position.z = start.pose.position.z + z_gap*frac
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")
if jagged:
for i,p in enumerate(poses):
if i%2 == 0:
poses[i] = self.pose_frame_move(poses[i], 0, 0.007,0)
else:
poses[i] = self.pose_frame_move(poses[i], 0, -0.007,0)
#return poses
return self.fill_ik_traj(poses, dist, duration, tot_points)
def check_trajectory(self, traj):
traj_angles = [list(traj.points[i].positions) for i in range(len(traj.points))]
for i in range(7):
traj_max = max(np.abs(np.diff(traj_angles,axis=0)[i]))
traj_mean = 4*np.mean(np.abs(np.diff(traj_angles,axis=0)[i]))
print 'traj: max: ', traj_max, 'mean: ', traj_mean
if traj_max >= traj_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 False
else:
return True
def fill_ik_traj(self, poses, dist=None, duration=None, tot_points=None):
if dist is None:
dist = self.calc_dist(poses[0], poses[-1])
if duration is None:
duration = dist*30
if tot_points is None:
tot_points = 500*dist
ik_fracs = np.linspace(0, 1, len(poses)) #A list of fractional positions along course to evaluate ik
req = self.form_ik_request(poses[0])
ik_goal = self.ik_pose_proxy(req)
seed = ik_goal.solution.joint_state.position
ik_points = [[0]*7 for i in range(len(poses))]
for i, p in enumerate(poses):
request = self.form_ik_request(p)
request.ik_request.ik_seed_state.joint_state.position = seed
ik_goal = self.ik_pose_proxy(request)
ik_points[i] = ik_goal.solution.joint_state.position
seed = ik_goal.solution.joint_state.position # seed the next ik w/previous points results
try:
ik_points = np.array(ik_points)
except:
print "Value error"
print 'ik_points: ', ik_points
print 'ik_points_array: ', ik_points
ang_fracs = np.linspace(0,1, tot_points)
diff = np.max(np.abs(np.diff(ik_points,1,0)),0)
print 'diff: ', diff
for i in xrange(len(ik_points)):
if ik_points[i,4]<0.:
ik_points[i,4] += 2*math.pi
if any(ik_points[:,4]>3.8):
print "OVER JOINT LIMITS!"
ik_points[:,4] -= 2*math.pi
# Linearly interpolate angles between ik-defined points (non-linear in cartesian space, but this is reduced from dense ik sampling along linear path. Used to maintain large number of trajectory points without running IK on every one.
angle_points = np.zeros((7, tot_points))
for i in xrange(7):
angle_points[i,:] = np.interp(ang_fracs, ik_fracs, ik_points[:,i])
#Build Joint Trajectory from angle positions
traj = JointTrajectory()
traj.header.frame_id = poses[0].header.frame_id
traj.joint_names = self.ik_info.kinematic_solver_info.joint_names
#print 'angle_points', len(angle_points[0]), angle_points
for i in range(len(angle_points[0])):
traj.points.append(JointTrajectoryPoint())
traj.points[i].positions = angle_points[:,i]
traj.points[i].velocities = [0]*7
traj.points[i].time_from_start = rospy.Duration(ang_fracs[i]*duration)
angles_safe = self.check_trajectory(traj)
if angles_safe:
print "Check Passed: Angles Safe"
return traj
else:
print "Check Failed: Unsafe Angles"
return None
def build_follow_trajectory(self, traj):
# Build 'Follow Joint Trajectory' goal from trajectory (includes tolerances for error)
traj_goal = FollowJointTrajectoryGoal()
traj_goal.trajectory = traj
tolerance = JointTolerance()
tolerance.position = 0.05
tolerance.velocity = 0.1
traj_goal.path_tolerance = [tolerance for i in range(len(traj.points))]
traj_goal.goal_tolerance = [tolerance for i in range(len(traj.points))]
traj_goal.goal_time_tolerance = rospy.Duration(3)
return traj_goal
def move_torso(self, pos):
rospy.loginfo("Moving Torso to reach arm goal")
goal_out = SingleJointPositionGoal()
goal_out.position = pos
finished_within_time = False
self.torso_client.send_goal(goal_out)
finished_within_time = self.torso_client.wait_for_result(rospy.Duration(45))
if not (finished_within_time):
self.torso_client.cancel_goal()
self.wt_log_out.publish(data="Timed out moving torso")
rospy.loginfo("Timed out moving torso")
return False
else:
state = self.torso_client.get_state()
result = self.torso_client.get_result()
if (state == 3): #3 == SUCCEEDED
rospy.loginfo("Torso Action Succeeded")
self.wt_log_out.publish(data="Move Torso Succeeded")
return True
else:
rospy.loginfo("Move Torso Failed")
rospy.loginfo("Failure Result: %s" %result)
self.wt_log_out.publish(data="Move Torso Failed")
return False
def check_torso(self, request):
rospy.loginfo("Checking Torso")
goal_z = request.ik_request.pose_stamped.pose.position.z
#print "Goal z: %s" %goal_z
self.torso_state_lock.acquire()
torso_pos = self.torso_state.actual.positions[0]
self.torso_state_lock.release()
spine_range = [self.torso_min - torso_pos, self.torso_max - torso_pos]
rospy.loginfo("Spine Range: %s" %spine_range)
#Check for exact solution if goal can be made level with spine (possible known best case first)
if goal_z >= spine_range[0] and goal_z <= spine_range[1]:
rospy.loginfo("Goal within spine movement range")
request.ik_request.pose_stamped.pose.position.z = 0;
streach_goal = goal_z
elif goal_z > spine_range[1]:#Goal is above possible shoulder height
rospy.loginfo("Goal above spine movement range")
request.ik_request.pose_stamped.pose.position.z -= spine_range[1]
streach_goal = spine_range[1]
else:#Goal is below possible shoulder height
rospy.loginfo("Goal below spine movement range")
request.ik_request.pose_stamped.pose.position.z -= spine_range[0]
streach_goal = spine_range[0]
#print "Checking optimal position, which gives: \r\n %s" %request.ik_request.pose_stamped.pose.position
ik_goal = self.ik_pose_proxy(request)
if ik_goal.error_code.val ==1:
rospy.loginfo("Goal can be reached by moving spine")
self.wt_log_out.publish(data="Goal can be reached by moving spine")
#print "Streach Goal: %s" %streach_goal
else:
rospy.loginfo("Goal cannot be reached, even using spine movement")
return [False, request.ik_request.pose_stamped]
#Find nearest working solution (so you don't wait for full-range spine motions all the time, but move incrementally
trial = 1
while True:
request.ik_request.pose_stamped.pose.position.z = goal_z - 0.1*trial*streach_goal
ik_goal = self.ik_pose_proxy(request)
if ik_goal.error_code.val == 1:
self.wt_log_out.publish(data="Using torso to reach goal")
rospy.loginfo("Using Torso to reach goal")
#print "Acceptable modified Pose: \r\n %s" %request.ik_request.pose_stamped.pose.position
streached = self.move_torso(torso_pos + 0.1*trial*streach_goal)
return [True, request.ik_request.pose_stamped]
else:
if trial < 10:
trial += 1
print "Trial %s" %trial
else:
return [False, request.ik_request.pose_stamped]
def fast_move(self, ps, time=0.):
ik_goal = self.ik_pose_proxy(self.form_ik_request(ps))
if ik_goal.error_code.val == 1:
point = JointTrajectoryPoint()
point.positions = ik_goal.solution.joint_state.position
self.send_traj_point(point, time)
else:
rospy.logerr("FAST MOVE IK FAILED!")
def blind_move(self, ps, duration = None):
(reachable, ik_goal) = self.full_ik_check(ps)
if reachable:
self.send_joint_angles(ik_goal.solution.joint_state.position, duration)
def ik_check(self, ps):
req = self.form_ik_request(ps)
ik_goal = self.ik_pose_proxy(req)
if ik_goal.error_code.val == 1:
return (True, ik_goal)
else:
return (False, ik_goal)
def full_ik_check(self, ps):
#print "Blind Move Command Received:\r\n %s" %ps.pose.position
req = self.form_ik_request(ps)
ik_goal = self.ik_pose_proxy(req)
if ik_goal.error_code.val == 1:
self.dist = self.calc_dist(ps)
#print "Initial IK Good, sending goal: %s" %ik_goal
return (True, ik_goal)
else:
# print "Initial IK Bad, finding better solution"
(torso_succeeded, pos) = self.check_torso(req)
if torso_succeeded:
# print "Successful with Torso, sending new position"
# print "Fully reachable with torso adjustment: New Pose:\r\n %s" %pos
self.dist = self.calc_dist(pos)
ik_goal = self.ik_pose_proxy(self.form_ik_request(pos))# From pose, get request, get ik
return (True, ik_goal)
else:
rospy.loginfo("Incrementing Reach")
percent = 0.9
while percent > 0.01:
print "Percent: %s" %percent
#print "Trying to move %s percent of the way" %percent
goal_pos = req.ik_request.pose_stamped.pose.position
#print "goal_pos: \r\n %s" %goal_pos
curr_pos = self.curr_pose()
curr_pos = curr_pos.pose.position
#print "curr_pos: \r\n %s" %curr_pos
req.ik_request.pose_stamped.pose.position.x = curr_pos.x + percent*(goal_pos.x-curr_pos.x)
req.ik_request.pose_stamped.pose.position.y = curr_pos.y + percent*(goal_pos.y-curr_pos.y)
req.ik_request.pose_stamped.pose.position.z = curr_pos.z + percent*(goal_pos.z-curr_pos.z)
#print "Check torso for part-way pose:\r\n %s" %req.ik_request.pose_stamped.pose.position
(torso_succeeded, pos) = self.check_torso(req)
#print "Pos: %s" %pos
if torso_succeeded:
print "Successful with Torso, sending new position"
self.dist = self.calc_dist(pos)
#print "new pose from torso movement: \r\n %s" %pos.pose.position
ik_goal = self.ik_pose_proxy(self.form_ik_request(pos))# From pose, get request, get ik
return (True, ik_goal)
else:
rospy.loginfo("Torso Could not reach goal")
ik_goal = self.ik_pose_proxy(req)
if ik_goal.error_code.val == 1:
rospy.loginfo("Initial Goal Out of Reach, Moving as Far as Possible")
self.wt_log_out.publish(data="Initial Goal Out of Reach, Moving as Far as Possible")
self.dist = self.calc_dist(req.ik_request.pose_stamped)
return (True, ik_goal)
else:
percent -= 0.1
rospy.loginfo("IK Failed: Error Code %s" %str(ik_goal.error_code))
self.wt_log_out.publish(data="Inverse Kinematics Failed: Goal Out of Reach.")
return (False, ik_goal)
def calc_dist(self, ps1, ps2=None):
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(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(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 form_ik_request(self, ps):
#print "forming IK request for :%s" %ps
req = GetPositionIKRequest()
req.timeout = rospy.Duration(5)
req.ik_request.pose_stamped = ps
req.ik_request.ik_link_name = self.ik_info.kinematic_solver_info.link_names[-1]
req.ik_request.ik_seed_state.joint_state.name = self.ik_info.kinematic_solver_info.joint_names
#self.joint_state_lock.acquire()
req.ik_request.ik_seed_state.joint_state.position = self.joint_state_act.positions
#self.joint_state_lock.release()
return req
def send_joint_angles(self, angles, duration = None):
point = JointTrajectoryPoint()
point.positions = angles
self.send_traj_point(point, duration)
def send_traj_point(self, point, time=None):
if time is None:
point.time_from_start = rospy.Duration(max(20*self.dist, 4))
else:
point.time_from_start = rospy.Duration(time)
joint_traj = JointTrajectory()
joint_traj.header.stamp = rospy.Time.now()
joint_traj.header.frame_id = '/torso_lift_link'
joint_traj.joint_names = self.ik_info.kinematic_solver_info.joint_names
joint_traj.points.append(point)
joint_traj.points[0].velocities = [0,0,0,0,0,0,0]
#print joint_traj
rarm_goal = JointTrajectoryGoal()
rarm_goal.trajectory = joint_traj
self.l_arm_traj_client.send_goal(rarm_goal)
#rospy.sleep(point.time_from_start)
#self.l_arm_command.publish(joint_traj)
def move_arm_to(self, goal_in):
rospy.loginfo("composing move_left_arm goal")
goal_out = arm_navigation_msgs.msg.MoveArmGoal()
goal_out.motion_plan_request.group_name = "left_arm"
goal_out.motion_plan_request.num_planning_attempts = 1
goal_out.motion_plan_request.planner_id = ""
goal_out.planner_service_name = "ompl_planning/plan_kinematic_path"
goal_out.motion_plan_request.allowed_planning_time = rospy.Duration(5.0)
pos = arm_navigation_msgs.msg.PositionConstraint()
pos.header.frame_id = goal_in.header.frame_id
pos.link_name="l_wrist_roll_link"
pos.position = goal_in.pose.position
pos.constraint_region_shape.type = 0
pos.constraint_region_shape.dimensions=[0.01]
pos.constraint_region_orientation = Quaternion(0,0,0,1)
pos.weight = 1
goal_out.motion_plan_request.goal_constraints.position_constraints.append(pos)
ort = arm_navigation_msgs.msg.OrientationConstraint()
ort.header.frame_id=goal_in.header.frame_id
ort.link_name="l_wrist_roll_link"
ort.orientation = goal_in.pose.orientation
ort.absolute_roll_tolerance = ort.absolute_pitch_tolerance = ort.absolute_yaw_tolerance = 0.02
ort.weight = 0.5
goal_out.motion_plan_request.goal_constraints.orientation_constraints.append(ort)
rospy.loginfo("Sending command to move arm with planning")
self.wt_log_out.publish(data="Sending command to move arm with planning.")
finished_within_time = False
self.move_left_arm_client.send_goal(goal_out)
finished_within_time = self.move_left_arm_client.wait_for_result(rospy.Duration(60))
if not (finished_within_time):
self.move_left_arm_client.cancel_goal()
self.wt_log_out.publish(data="Timed out achieving left arm goal pose")
rospy.loginfo("Timed out achieving left arm goal pose")
return False
else:
state = self.move_left_arm_client.get_state()
result = self.move_left_arm_client.get_result()
if (state == 3): #3 == SUCCEEDED
rospy.loginfo("Action Finished: %s" %state)
self.wt_log_out.publish(data="Move Left Arm with Motion Planning: %s" %self.move_arm_error_dict[result.error_code.val])
return True
else:
if result.error_code.val == 1:
rospy.loginfo("Move_left_arm_action failed: Unable to plan a path to goal")
self.wt_log_out.publish(data="Move Left Arm with Motion Planning: Failed: Unable to plan a path to the goal")
elif result.error_code.val == -31:
(torso_succeeded, pos) = self.check_torso(self.form_ik_request(goal_in))
if torso_succeeded:
rospy.loginfo("IK Failed in Current Position. Adjusting Height and Retrying")
self.wt_log_out.publish(data="IK Failed in Current Position. Adjusting Height and Retrying")
self.move_arm_to(pos)
else:
rospy.loginfo("Move_left_arm action failed: %s" %state)
rospy.loginfo("Failure Result: %s" %result)
self.wt_log_out.publish(data="Move Left Arm with Motion Planning and Torso Check Failed: %s" %self.move_arm_error_dict[result.error_code.val])
else:
rospy.loginfo("Move_left_arm action failed: %s" %state)
rospy.loginfo("Failure Result: %s" %result)
self.wt_log_out.publish(data="Move Left Arm with Motion Planning: Failed: %s" %self.move_arm_error_dict[result.error_code.val])
return False
rospy.loginfo('loading calibration parameters into namespace {}'.format(
rospy.get_namespace()))
calib.to_parameters()
orig = calib.transformation.header.frame_id # tool or base link
dest = calib.transformation.child_frame_id # tracking_base_frame
transformer = TransformerROS()
result_tf = calib.transformation.transform
transl = result_tf.translation.x, result_tf.translation.y, result_tf.translation.z
rot = result_tf.rotation.x, result_tf.rotation.y, result_tf.rotation.z, result_tf.rotation.w
cal_mat = transformer.fromTranslationRotation(transl, rot)
if inverse:
cal_mat = tfs.inverse_matrix(cal_mat)
orig, dest = dest, orig
translation = tfs.translation_from_matrix(cal_mat)
rotation = tfs.quaternion_from_matrix(cal_mat)
rospy.loginfo('publishing transformation ' + orig + ' -> ' + dest + ':\n' +
str((translation, rotation)))
broad = TransformBroadcaster()
rate = rospy.Rate(50)
while not rospy.is_shutdown():
broad.sendTransform(translation, rotation, rospy.Time.now(), dest,
orig) # takes ..., child, parent
rate.sleep()
class PublishTf:
def __init__(self):
self.br = TransformBroadcaster()
self.pub_freq = 20.0
self.parent_frame_id = "world"
self.child1_frame_id = "reference1"
self.child2_frame_id = "reference2"
self.child3_frame_id = "reference3"
self.child4_frame_id = "reference4"
rospy.Timer(rospy.Duration(1 / self.pub_freq), self.reference2)
rospy.Timer(rospy.Duration(1 / self.pub_freq), self.reference3)
rospy.Timer(rospy.Duration(1 / self.pub_freq), self.reference4)
rospy.sleep(1.0)
def reference2(self, event):
self.check_for_ctrlc()
self.pub_tf(self.child1_frame_id, self.child2_frame_id, [1, 0, 0])
def reference3(self, event):
self.check_for_ctrlc()
self.pub_tf(self.child1_frame_id, self.child3_frame_id, [math.sin(rospy.Time.now().to_sec()), 0, 0])
def reference4(self, event):
self.check_for_ctrlc()
self.pub_tf(self.child1_frame_id, self.child4_frame_id, [math.sin(rospy.Time.now().to_sec()),
math.cos(rospy.Time.now().to_sec()), 0])
def pub_tf(self, parent_frame_id, child1_frame_id, xyz=[0, 0, 0], rpy=[0, 0, 0]):
self.check_for_ctrlc()
start = rospy.Time.now()
try:
self.br.sendTransform((xyz[0], xyz[1], xyz[2]), transformations.quaternion_from_euler(
rpy[0], rpy[1], rpy[2]), rospy.Time.now(), child1_frame_id, parent_frame_id)
except rospy.ROSException:
rospy.logdebug("could not send transform")
stop = rospy.Time.now()
if (stop-start).to_sec() > 1/self.pub_freq:
rospy.logwarn("Publishing tf took longer than specified loop rate " + str((stop-start).to_sec()) + ", should be less than " + str(1/self.pub_freq))
def pub_line(self, length=1, time=1):
rospy.loginfo("Line")
rate = rospy.Rate(int(self.pub_freq))
for i in range((int(self.pub_freq * time / 2) + 1)):
t = i / self.pub_freq / time * 2
self.pub_tf(self.parent_frame_id, self.child1_frame_id, [t * length, 0, 0])
rate.sleep()
for i in range((int(self.pub_freq * time / 2) + 1)):
t = i / self.pub_freq / time * 2
self.pub_tf(self.parent_frame_id, self.child1_frame_id, [(1 - t) * length, 0, 0])
rate.sleep()
def pub_circ(self, radius=1, time=1):
rospy.loginfo("Circ")
rate = rospy.Rate(int(self.pub_freq))
for i in range(int(self.pub_freq * time) + 1):
t = i / self.pub_freq / time
self.pub_tf(self.parent_frame_id, self.child1_frame_id, [-radius * math.cos(2 * math.pi * t) + radius,
-radius * math.sin(2 * math.pi * t),
0])
rate.sleep()
def pub_quadrat(self, length=1, time=1):
rospy.loginfo("Quadrat")
rate = rospy.Rate(int(self.pub_freq))
for i in range((int(self.pub_freq * time / 4) + 1)):
t = i / self.pub_freq / time * 4
self.pub_tf(self.parent_frame_id, self.child1_frame_id, [t * length, 0, 0])
rate.sleep()
for i in range((int(self.pub_freq * time / 4) + 1)):
t = i / self.pub_freq / time * 4
self.pub_tf(self.parent_frame_id, self.child1_frame_id, [length, t * length, 0])
rate.sleep()
for i in range((int(self.pub_freq * time / 4) + 1)):
t = i / self.pub_freq / time * 4
self.pub_tf(self.parent_frame_id, self.child1_frame_id, [(1 - t) * length, length, 0])
rate.sleep()
for i in range((int(self.pub_freq * time / 4) + 1)):
t = i / self.pub_freq / time * 4
self.pub_tf(self.parent_frame_id, self.child1_frame_id, [0, (1 - t) * length, 0])
rate.sleep()
def check_for_ctrlc(self):
if rospy.is_shutdown():
sys.exit()
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 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
"""
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,
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
# 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()
# 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)
# 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 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 = []
# 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) """
pass
# 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 = 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
TODO: if you want to learn a lot about tf, reimplement this... I can provide
you with some hints as to what is going on here. """
(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.tf_listener.waitForTransform(self.base_frame,
self.odom_frame, msg.header.stamp,
rospy.Duration(1.0))
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 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 = "base_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.08 # guess for how inaccurate lidar readings 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("particlecloud", PoseArray, queue_size=10)
self.marker_pub = rospy.Publisher("markers", 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
self.map_server = rospy.ServiceProxy('static_map', GetMap)
self.map = self.map_server().map
# for now we have commented out the occupancy field initialization until you can successfully fetch the map
self.occupancy_field = OccupancyField(self.map)
self.initialized = True
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 = []
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_tuple = tf.transformations.quaternion_from_euler(0,0,theta)
self.robot_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]))
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 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):
""" 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()
newParticles = []
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
newParticles.append(deepcopy(particle))
break
self.particle_cloud = newParticles
def update_particles_with_laser(self, msg):
""" Updates the particle weights in response to the scan contained in the msg """
for particle in self.particle_cloud:
tot_prob = 0
for index, scan in enumerate(msg.ranges):
x,y = self.transform_scan(particle,scan,index)
# transform scan to view of the particle
d = self.occupancy_field.get_closest_obstacle_distance(x,y)
# calculate nearest distance to particle's scan (should be near 0 if it's on robot)
tot_prob += math.exp((-d**2)/(2*self.sigma**2))
# add probability (0 to 1) to total probability
tot_prob = tot_prob/len(msg.ranges)
# normalize total probability back to 0-1
particle.w = tot_prob
# assign particles weight
def transform_scan(self, particle, distance, theta):
""" Calculates the x and y of a scan from a given particle
particle: Particle object
distance: scan distance (from ranges)
theta: scan angle (range index)
"""
return (particle.x + distance * math.cos(math.radians(particle.theta + theta)),
particle.y + distance * math.sin(math.radians(particle.theta + theta)))
@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))
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 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()
@staticmethod
def convert_translation_rotation_to_pose(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])
@staticmethod
def convert_theta_to_quaternion(theta):
return t.quaternion_from_euler(0, 0, theta)
@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))
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 rotate_2d_vector(self, vec, angle):
# angle = math.radians(30)
# vec = [1, 0]
c, s = math.cos(angle), math.sin(angle)
R = np.array(((c, -s), (s, c)))
return R.dot(vec)
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 """
(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 FakeHuman(EventSimulator):
'''
Simulating the perceptual data we would get from a human.
The human is symbolized by a red ball at roughly human size
relative to the robot in the view.
Given a schedule of poses for the human, instances of
this class move the human through the scheduled timed poses.
A schedule looks like this:
motionSchedule = OrderedDict();
motionSchedule[3] = {'target': [2,0], 'speed': [0.5,0.0]};
motionSchedule[6] = {'target': [3,0], 'speed': [0.5,0.0]};
motionSchedule[9] = {'target': [4,0], 'speed': [0.5,0.0]};
Here the motionSchedule keys (3,6, and 9) are seconds from program start.
the 'target' numbers are meters of distance from the robot, and the
two speed components are meters/sec in the x and y direction, respectively.
When the scheduled pose sequence is completed, it starts over.
Start the sequence with a call to a FakeHuman's start() method, passing
it a schedule. Stop the motions via a call to the stop() method.
'''
def __init__(self):
self.tfBroadcaster = TransformBroadcaster()
q = transformations.quaternion_from_euler(0,0,pi)
self.humanPose = Pose(Point(5, 0, 1.65), Quaternion(*q))
self.markerPublisher = rospy.Publisher('visualization_marker', Marker)
rospy.loginfo('Initialized.')
self.startTime = time.time()
rospy.init_node('fake_human_node', anonymous=True)
def start(self, motionSchedule):
self.motionSchedule = motionSchedule;
self.motionScheduleKeys = motionSchedule.keys();
# Current index into the motion schedule keys:
self.motionScheduleIndex = 0;
threadSchedule = OrderedDict();
# Create a schedule that calls the callback function (i.e. self.update())
# at 0.2 seconds, and then repeats. The None is the argument passed
# to self.update().
threadSchedule[0.2] = None;
self.startTime = time.time();
#******super(FakeHuman, self).start(threadSchedule, self.update, repeat=True, callbackInterval=FRAME_INTERVAL);
super(FakeHuman, self).start(motionSchedule, self.update, repeat=True, callbackInterval=FRAME_INTERVAL);
def publishTFPose(self, pose, name, parent):
if (pose != None) and (not rospy.is_shutdown()):
try:
self.tfBroadcaster.sendTransform((pose.position.x, pose.position.y, pose.position.z),
(pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w),
rospy.Time.now(), name, parent)
except rospy.ROSException:
rospy.loginfo("Fake human topic was closed during publish. Likely due to explicit shutdown request.")
def getPlanarDistance(self, pose):
'''
Compute distance of a given pose from its origin in a 2d plane.
@param pose: pose to examine
@type pose: Pose
@return: linear distance
@rType: float
'''
vec = numpy.array((pose.position.x, pose.position.y))
self.planarDistance = numpy.linalg.norm(vec)
return self.planarDistance
def getDiff(self, target, current, speed):
diff = target - current
if diff > speed:
return speed, False
elif diff < -speed:
return -speed, False
return diff, True
def moveTowardsTarget(self, speed):
'''
Called from update(). Moves the rviz representation of the human towards
a target position.
@param speed: motion speed
@type speed: float
@return: whether fake human moved closer to the robot, or away from it
@rtype: MotionDirection
'''
initialDistanceFromRobot = self.getPlanarDistance(self.humanPose);
xDiff, xReached = self.getDiff(self.target[0], self.humanPose.position.x, speed[0])
yDiff, yReached = self.getDiff(self.target[1], self.humanPose.position.y, speed[1])
self.humanPose.position.x += xDiff
self.humanPose.position.y += yDiff
newDistanceFromRobot = self.getPlanarDistance(self.humanPose)
if (newDistanceFromRobot - initialDistanceFromRobot) > 0:
return MotionDirection.TOWARDS_ROBOT
else:
return MotionDirection.AWAY_FROM_ROBOT
def update(self, dummy):
'''
Called repeatedly from the EventSimulator thread. Moves the Rviz human towards
various locations in small increments. Locations, speed, and move times are
taken from self.motionSchedule, which was passed into the start() method.
Example schedule is the following dict (keys are fractional seconds)::
motionSchedule[2] = None;
motionSchedule[5] = {target: [2,-1], speed: [0.1,0.03]};
motionSchedule[10] = None;
motionSchedule[12] = {target: [4,-2], speed: [.1,.055]};
motionSchedule[16] = None;
motionSchedule[20] = {target: [0.6,.5], speed: [.08,.1]};
motionSchedule[27] = None;
@param dummy: EventSimulator calls with an argument, which is None in our case, and unused.
@type dummy: None.
'''
timePassed = time.time() - self.startTime;
# Get current motion's upper time bound:
keyframeTime = self.motionScheduleKeys[self.motionScheduleIndex];
# Is time later than that end time of current motion?
while timePassed >= keyframeTime:
# Grab the next schedule entry:
self.motionScheduleIndex += 1;
if self.motionScheduleIndex >= len(self.motionSchedule):
# Ran out of schedule entries. Start over:
self.motionScheduleIndex = 0;
self.startTime = time.time();
timePassed = time.time() - self.startTime;
break;
else:
# Grab the new schedule entry's upper time bound (the dict key):
keyframeTime = self.motionScheduleKeys[self.motionScheduleIndex];
try:
# Get motion x/y and x/y speeds:
keyframeTarget = self.motionSchedule[keyframeTime]['target'];
keyframeSpeed = self.motionSchedule[keyframeTime]['speed'];
except TypeError:
# Motion schedule value for this entry is None; do nothing:
self.visualizeFakeHuman();
return;
self.target = keyframeTarget;
targetPose = Pose(Point(keyframeTarget[0],keyframeTarget[1],0), Quaternion(0,0,0,0))
self.targetDistance = self.getPlanarDistance(targetPose);
#********************
#print(str(timePassed) + "s. Target: " + str(self.targetDistance) + "m. CurrDist: " + str(self.getPlanarDistance(self.humanPose)));
#********************
# Move the human a little bit (we ignore the move direction):
motionDirection = self.moveTowardsTarget(keyframeSpeed);
self.visualizeFakeHuman();
# Return a copy of the human pose (because self.humanPose
# is continuously updated:
return self.getHumanPose();
def visualizeFakeHuman(self):
# Visualize the fake human:
if (self.humanPose is not None):
self.publishTFPose(self.humanPose, 'fake_human', 'odom_combined')
m = Marker(type=Marker.SPHERE, id=0, lifetime=rospy.Duration(2), pose=self.humanPose,
scale=Vector3(0.3,0.3,0.3), header=Header(frame_id='odom_combined'),
color=ColorRGBA(1.0, 0.0, 0.0, 0.8))
try:
self.markerPublisher.publish(m)
except rospy.ROSException:
rospy.loginfo("Publish to a closed topic. Likely due to explicit shutdown request")
def getHumanPose(self):
'''
Make a snapshot copy of the (constantly updated) human pose.
'''
humanPoint = Point(self.humanPose.position.x, self.humanPose.position.y, self.humanPose.position.z)
humanQuat = Quaternion(self.humanPose.orientation.x, self.humanPose.orientation.y, self.humanPose.orientation.z ,self.humanPose.orientation.w);
return Pose(humanPoint, humanQuat)
