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)