def tf_from_pose(self, pose_msg): # Update message timestamp self.tf_msg.header.stamp = rospy.Time.now() # Update translation self.tf_msg.transform.translation.x = pose_msg.position.x self.tf_msg.transform.translation.y = pose_msg.position.y self.tf_msg.transform.translation.z = pose_msg.position.z # Update rotation self.tf_msg.transform.rotation.x = pose_msg.orientation.x self.tf_msg.transform.rotation.y = pose_msg.orientation.y self.tf_msg.transform.rotation.z = pose_msg.orientation.z self.tf_msg.transform.rotation.w = pose_msg.orientation.w if sum ([self.tf_msg.transform.rotation.x, self.tf_msg.transform.rotation.y, self.tf_msg.transform.rotation.z, self.tf_msg.transform.rotation.w]) != 0.0: if self.invert == "True": # Create transformerROS and add our transform transformer = TransformerROS() transformer.setTransform(self.tf_msg) # Lookup the transform (trans, rot) = transformer.lookupTransform(self.tf_msg.header.frame_id, self.tf_msg.child_frame_id, rospy.Time(0)) # Create transform object transform = tft.concatenate_matrices(tft.translation_matrix(trans), tft.quaternion_matrix(rot)) # Invert inverse_tf = tft.inverse_matrix(transform) # Get translation, rotation vectors back out translation = tft.translation_from_matrix(inverse_tf) quaternion = tft.quaternion_from_matrix(inverse_tf) # Get RPY rpy = tft.euler_from_quaternion(quaternion) rpy_new = [rpy[0], rpy[1], -rpy[2]] # Back to quaternion quaternion = tft.quaternion_from_euler(rpy_new[0], rpy_new[1], rpy_new[2]) # Update translation self.tf_msg.transform.translation.x = translation[0] self.tf_msg.transform.translation.y = -translation[1] self.tf_msg.transform.translation.z = translation[2] # Update rotation self.tf_msg.transform.rotation.x = quaternion[0] self.tf_msg.transform.rotation.y = quaternion[1] self.tf_msg.transform.rotation.z = quaternion[2] self.tf_msg.transform.rotation.w = quaternion[3] # Publish transform self.broadcaster.sendTransform(self.tf_msg)
def get_transformation(frame_from='/base_link', frame_to='/local_map', time_from=None, time_to=None, static_frame=None, tf_listener=None, tf_ros=None): if tf_listener is None: tf_listener = TransformListener() if tf_ros is None: tf_ros = TransformerROS() try: if time_from is None or time_to is None: # tf_listener.waitForTransform(frame_from, frame_to, rospy.Time(), rospy.Duration(1.0)) (trans, rot) = tf_listener.lookupTransform(frame_to, frame_from, rospy.Time(0)) else: # tf_listener.waitForTransformFull(frame_from, time_from, frame_to, time_to, static_frame, rospy.Duration(1.0)) (trans, rot) = tf_listener.lookupTransformFull(frame_to, time_to, frame_from, time_from, static_frame) except (LookupException, ConnectivityException, ExtrapolationException): rospy.logerr("exception, from %s to %s frame may not have setup!", frame_from, frame_to) return None, None, None, None # pose.pose.orientation.w = 1.0 # Neutral orientation # tf_pose = self.tf_listener_.transformPose("/world", pose) # R_local = quaternion_matrix(tf_pose.pose.orientation) T = tf_ros.fromTranslationRotation(trans, rot) euler = euler_from_quaternion(rot) # print "Position of the pose in the local map:" # print trans, euler return T, trans, rot, euler
def get_transform_from_pose(pose, tf_ros=None): """ from pose to origin (assumed 0,0,0) """ if tf_ros is None: tf_ros = TransformerROS() translation = (pose.position.x, pose.position.y, pose.position.z) rotation = (pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w) T_pose_to_origin = tf_ros.fromTranslationRotation(translation, rotation) return T_pose_to_origin
def __init__(self, num_particles=100, vis_noise=10, ultra_noise=100, mag_noise=37, linear_noise=.002, angular_noise=2.3, ar_noise=10, ar_resample_rate=10, ar_resample=True): print("Starting a particle filer with %d particles" % num_particles) self.filename = os.path.expanduser( "~/Dropbox/thesis_data/" + time.strftime('%Y_%m_%d_%H_%M_%S')) #in the format YYYYMMDDHHMMSS self.fp = open(self.filename + ".txt", "w") self.fp_part = open(self.filename + "_part.txt", "w") self.fp_ar = open(self.filename + "_ar.txt", "w") self.fp_alt = open(self.filename + "_alt.txt", "w") self.fp_sensor = open(self.filename + "_sensor.txt", "w") self.num_particles = num_particles self.vis_noise = vis_noise self.ultra_noise = ultra_noise self.mag_noise = mag_noise self.linear_noise = linear_noise self.angular_noise = angular_noise self.ar_noise = ar_noise self.ar_resample_rate = ar_resample_rate self.ar_resample = ar_resample self.rotY = 0 self.rotX = 0 self.start_mag_heading = 0 self.start_gyr_heading = 0 self.gyr_theta = 0 self.particle_list = [] self.weight_dict = dict() self.first_propagate = True self.first_correct = True self.step = 0 self.est = particle(self) self.acc_est = particle(self) #Estimation using acc and gyr self.vis_est = particle( self) #Estimation using vis odometry and ultrasound self.tag_est = particle(self) #Estimation using visual tags for i in range(num_particles): self.particle_list.append(particle(self)) self.est_pose = Pose() self.est_pub = rospy.Publisher('pf_localization', Pose) self.listener = TransformListener() self.transformer = TransformerROS() self.publish_pose()
def invert_pose(pose): transformer = TransformerROS() transform = transform_from_pose('frame1', 'frame2', pose) transformer.setTransform(transform) frame1_origin_frame1 = pose_stamped('frame1', [0.0, 0.0, 0.0], [0.0, 0.0, 0.0, 1.0]) frame1_origin_frame2 = transformer.transformPose('frame2', frame1_origin_frame1) return frame1_origin_frame2.pose
def get_ik(pose): matrix = TransformerROS() # The orientation of /tool0 will be constant q = quaternion_from_euler(0, 3.14, 1.57) matrix2 = matrix.fromTranslationRotation((pose[0]*(-1), pose[1]*(-1), pose[2]), (q[0], q[1], q[2], q[3])) th = invKine(matrix2) sol1 = th[:, 2].transpose() joint_values_from_ik = np.array(sol1) joint_values = joint_values_from_ik[0, :] return joint_values.tolist()
def __init__(self): self.node_name = "Mocap Localization" self.global_map = ObstaclePoseList() self.first = True # process first time self.radius = 1 # to check whether the measurement obstacle is correspond to global map or not self.confi_threshold = 5 # confidence threshold, to determine whether to add point to map or not self.tf = TransformListener() self.transformer = TransformerROS() # Subscribers self.obstacle_list = rospy.Subscriber("/obstacle_list", ObstaclePoseList, self.call_back, queue_size=1) # Publishers self.rviz_pub = rospy.Publisher("/map_viz", Marker, queue_size=1) self.map_list = rospy.Publisher("/map_list", ObstaclePoseList, queue_size=1)
def __init__(self, map_frame, odom_frame, projection_namespace, kitchen_namespace): # Frame names of the map and odom frame self.map_frame = map_frame self.odom_frame = odom_frame # Namespaces self.projection_namespace = projection_namespace self.kitchen_namespace = kitchen_namespace # TF tf_stampeds and static_tf_stampeds of the Robot in the BulletWorld: # These are initialized with the function init_transforms_from_urdf and are # used to update the local transformer with the function update_local_transformer_from_btr self.tf_stampeds = [] self.static_tf_stampeds = [] self.local_transformer = TransformerROS(interpolate=True, cache_time=Duration(10.0)) self.init_local_tf_with_tfs_from_urdf()
def __init__(self): # Origin of ther intersection coordinate system is the center of stopline 0. tile_side_length = 0.61 lane_width = 0.205 stopline_thickness = 0.048 center_markings_thickness = 0.025 q = lane_width p = stopline_thickness s = center_markings_thickness self.stopline_poses = [] self.tf1 = TransformerROS() self.tf2 = TransformerROS() def add_stopline_transform(idx, position, angle): orientation = unit_vector( quaternion_from_euler(0, 0, angle, axes='sxyz')) pose = utils.pose(position, orientation) # Add transform for reference stopline poses child_frame = self.stopline_frame(idx) parent_frame = 'intersection' transform = utils.transform_from_pose('intersection', child_frame, pose) self.tf1.setTransform(transform) # Add transform for measured stopline poses child_frame = self.intersection_frame(idx) parent_frame = self.stopline_frame(idx) inverted_pose = utils.invert_pose(utils.pose( position, orientation)) transform = utils.transform_from_pose(parent_frame, child_frame, inverted_pose) self.tf2.setTransform(transform) add_stopline_transform(0, [0.0, 0.0, 0.0], 0) add_stopline_transform( 1, [-(q / 2.0 + s + q + p / 2.0), p / 2.0 + q / 2.0, 0.0], -np.pi / 2.0) add_stopline_transform( 2, [-(q / 2.0 + s + q / 2.0), p / 2.0 + 2 * q + s + p / 2.0, 0.0], -np.pi) add_stopline_transform( 3, [q / 2.0 + p / 2.0, p / 2.0 + q + s + q / 2.0, 0.0], -3.0 * np.pi / 2.0)
def transform_ar_tag(message): """ Input: message of type AlvarMarkers Transforms the encoded PoseStamped to the base frame and stores it in transformed_message """ global transformed_message if message.markers == []: return global counter if counter > 1: return # Create a TransformerROS to transform the AR tag poses we get t = TransformerROS(True, rospy.Duration(10.0)) # Wait for our transform and get it tf_listener.waitForTransform('/head_camera', '/base', rospy.Time(), rospy.Duration(5.0)) (trans, rot) = tf_listener.lookupTransform('/head_camera', '/base', rospy.Time(0)) # Create our TransformStamped object transform = TransformStamped() transform.child_frame_id = 'base' transform.header.frame_id = 'head_camera' transform.header.stamp = rospy.Time(0) transform.transform.translation.x = trans[0] transform.transform.translation.y = trans[1] transform.transform.translation.z = trans[2] transform.transform.rotation.x = rot[0] transform.transform.rotation.y = rot[1] transform.transform.rotation.z = rot[2] transform.transform.rotation.w = rot[3] # Set the transform for t t.setTransform(transform) pose = message.markers[0].pose # Assume one marker for now pose.header.frame_id = '/head_camera' transformed_message = t.transformPose('/base', pose) global board_x, board_y, board_z board_x = transformed_message.pose.position.x board_y = transformed_message.pose.position.y - 0.177 / 2 board_z = transformed_message.pose.position.z - 0.177 / 2 counter += 1
def __init__(self): pub = 0 self.tf = TransformListener() self.tf1 = TransformerROS() self.fdata = geometry_msgs.msg.TransformStamped() self.fdata_base = geometry_msgs.msg.TransformStamped() self.transform = tf.TransformBroadcaster() self.wrench = WrenchStamped() self.wrench_bl = WrenchStamped()
class Transformation(): def __init__(self): pub = 0 self.tf = TransformListener() self.tf1 = TransformerROS() self.fdata = geometry_msgs.msg.TransformStamped() self.fdata_base = geometry_msgs.msg.TransformStamped() self.transform = tf.TransformBroadcaster() self.wrench = WrenchStamped() self.wrench_bl = WrenchStamped() def wrench_cb(self,msg): self.wrench = msg.wrench self.transform.sendTransform((0,0,-0.025),quaternion_from_euler(3.14, 0, 3.665195102, 'rxyz'),rospy.Time.now(),'/ft_debug_link','/arm_7_link') self.fdata.transform.translation.x = self.wrench.force.x self.fdata.transform.translation.y = self.wrench.force.y self.fdata.transform.translation.z = self.wrench.force.z try: if self.tf.frameExists("/dummy_link") and self.tf.frameExists("ft_debug_link"): t = self.tf.getLatestCommonTime("/dummy_link", "/ft_debug_link") (transform_ee_base_position,transform_ee_base_quaternion) = self.tf.lookupTransform("/dummy_link", '/ft_debug_link', t) #print transform_ee_base_position #print transform_ee_base_quaternion #print self.tf1.fromTranslationRotation(transform_ee_base_position,transform_ee_base_quaternion) except(tf.LookupException,tf.ConnectivityException): print("TRANSFORMATION ERROR") sss.say(["error"]) #return 'failed' self.fdata_base.transform.translation.x =((self.tf1.fromTranslationRotation(transform_ee_base_position,transform_ee_base_quaternion)[0,0] * self.fdata.transform.translation.x)+ (self.tf1.fromTranslationRotation(transform_ee_base_position,transform_ee_base_quaternion)[0,1] * self.fdata.transform.translation.y)+ (self.tf1.fromTranslationRotation(transform_ee_base_position,transform_ee_base_quaternion)[0,2] * self.fdata.transform.translation.z)+ (self.tf1.fromTranslationRotation(transform_ee_base_position,transform_ee_base_quaternion)[0,3])) self.fdata_base.transform.translation.y =((self.tf1.fromTranslationRotation(transform_ee_base_position,transform_ee_base_quaternion)[1,0] * self.fdata.transform.translation.x)+ (self.tf1.fromTranslationRotation(transform_ee_base_position,transform_ee_base_quaternion)[1,1] * self.fdata.transform.translation.y)+ (self.tf1.fromTranslationRotation(transform_ee_base_position,transform_ee_base_quaternion)[1,2] * self.fdata.transform.translation.z)+ (self.tf1.fromTranslationRotation(transform_ee_base_position,transform_ee_base_quaternion)[1,3])) self.fdata_base.transform.translation.z =((self.tf1.fromTranslationRotation(transform_ee_base_position,transform_ee_base_quaternion)[2,0] * self.fdata.transform.translation.x)+ (self.tf1.fromTranslationRotation(transform_ee_base_position,transform_ee_base_quaternion)[2,1] * self.fdata.transform.translation.y)+ (self.tf1.fromTranslationRotation(transform_ee_base_position,transform_ee_base_quaternion)[2,2] * self.fdata.transform.translation.z)+ (self.tf1.fromTranslationRotation(transform_ee_base_position,transform_ee_base_quaternion)[2,3])) self.wrench_bl.wrench.force.y = self.fdata_base.transform.translation.y self.wrench_bl.wrench.force.x = self.fdata_base.transform.translation.x self.wrench_bl.wrench.force.z = self.fdata_base.transform.translation.z self.wrench_bl.header.stamp = rospy.Time.now(); self.pub.publish(self.wrench_bl)
def __init__(self): self.node_name = rospy.get_name() self.tf = TransformListener() self.transformer = TransformerROS() rospy.loginfo("[%s] Initializing " % (self.node_name)) #rospy.Subscriber("/pcl_points", PoseArray, self.call_back, queue_size=1, buff_size = 2**24) sub_pcl = message_filters.Subscriber("/pcl_points", PoseArray) sub_odom = message_filters.Subscriber("/odometry/ground_truth", Odometry) ats = ApproximateTimeSynchronizer((sub_pcl, sub_odom), queue_size=1, slop=0.1) ats.registerCallback(self.call_back) rospy.Subscriber("/move_base_simple/goal", PoseStamped, self.cb_new_goal, queue_size=1) self.pub_map = rospy.Publisher('/local_map', OccupancyGrid, queue_size=1) self.pub_rviz = rospy.Publisher("/wp_line", Marker, queue_size=1) self.pub_poses = rospy.Publisher("/path_points", PoseArray, queue_size=1) self.resolution = 0.25 self.robot = Pose() self.width = 200 self.height = 200 self.origin = Pose() self.local_map = OccupancyGrid() self.dilating_size = 6 self.wall_width = 3 self.start_planning = False self.transpose_matrix = None self.goal = [] self.astar = AStar() self.msg_count = 0 self.planning_range = 20 self.frame_id = "map"
def __init__(self): self.tfr = TransformerROS() self.projector = LaserProjection() self.outputTopic = rospy.get_param('output_topic', 'cloud') self.inputTopic = rospy.get_param('input_topic', 'scan') self.outputFrame = rospy.get_param('output_frame', 'base_link') self.publisher = rospy.Publisher(self.outputTopic, PointCloud2, queue_size=10) self.subscriber = rospy.Subscriber(self.inputTopic, LaserScan, self.laser_callback)
def __init__(self, num_particles=100, vis_noise=10, ultra_noise=100, mag_noise=37, linear_noise=.002, angular_noise=2.3, ar_noise=10, ar_resample_rate=10, ar_resample=True): print("Starting a particle filer with %d particles" % num_particles) self.filename = os.path.expanduser("~/Dropbox/thesis_data/" + time.strftime('%Y_%m_%d_%H_%M_%S')) #in the format YYYYMMDDHHMMSS self.fp = open(self.filename + ".txt", "w") self.fp_part = open(self.filename+"_part.txt", "w") self.fp_ar = open(self.filename+"_ar.txt", "w") self.fp_alt = open(self.filename+"_alt.txt", "w") self.fp_sensor = open(self.filename+"_sensor.txt", "w") self.num_particles = num_particles self.vis_noise = vis_noise self.ultra_noise = ultra_noise self.mag_noise = mag_noise self.linear_noise = linear_noise self.angular_noise = angular_noise self.ar_noise = ar_noise self.ar_resample_rate = ar_resample_rate self.ar_resample = ar_resample self.rotY = 0 self.rotX = 0 self.start_mag_heading = 0 self.start_gyr_heading = 0 self.gyr_theta = 0 self.particle_list = [] self.weight_dict = dict() self.first_propagate = True self.first_correct = True self.step = 0 self.est = particle(self) self.acc_est = particle(self) #Estimation using acc and gyr self.vis_est = particle(self) #Estimation using vis odometry and ultrasound self.tag_est = particle(self) #Estimation using visual tags for i in range(num_particles): self.particle_list.append(particle(self)) self.est_pose = Pose() self.est_pub = rospy.Publisher('pf_localization', Pose) self.listener = TransformListener() self.transformer = TransformerROS() self.publish_pose()
def __init__(self): # specify topic and data type self.sub_bbox_1 = rospy.Subscriber("camera1/detection/vision_objects", DetectedObjectArray, self.bbox_array_callback) self.sub_bbox_6 = rospy.Subscriber("camera6/detection/vision_objects", DetectedObjectArray, self.bbox_array_callback) self.sub_pcd = rospy.Subscriber("/points_raw", PointCloud2, self.pcd_callback) # self.sub_pose = rospy.Subscriber("/current_pose", PoseStamped, self.pose_callback) # Publisher self.pub_intersect_markers = rospy.Publisher( "/vision_objects_position_rviz", MarkerArray, queue_size=10) self.pub_plane_markers = rospy.Publisher("/estimated_plane_rviz", MarkerArray, queue_size=10) self.pub_plane = rospy.Publisher("/estimated_plane", Plane, queue_size=10) self.pub_pcd_inlier = rospy.Publisher("/points_inlier", PointCloud2, queue_size=10) self.pub_pcd_outlier = rospy.Publisher("/points_outlier", PointCloud2, queue_size=10) self.cam1 = camera_setup_1() self.cam6 = camera_setup_6() self.plane = Plane3D(-0.157, 0, 0.988, 1.9) self.plane_world = None self.plane_tracker = None self.sac = RANSAC(Plane3D, min_sample_num=3, threshold=0.22, iteration=200, method="MSAC") self.tf_listener = TransformListener() self.tfmr = Transformer() self.tf_ros = TransformerROS()
try: (trans, rot) = listener.lookupTransform(self.map_frame, self.robot_frame, t) except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException): continue transpose_matrix = transformer.fromTranslationRotation(trans, rot) # print(rot) p = np.array([0, 0, 0, 1]) new_p = np.dot(transpose_matrix, p) posestamped = PoseStamped() posestamped.header.frame_id = self.map_frame posestamped.header.stamp = t posestamped.pose.position.x = new_p[0] posestamped.pose.position.y = new_p[1] posestamped.pose.position.z = new_p[2] posestamped.pose.orientation.x = rot[0] posestamped.pose.orientation.y = rot[1] posestamped.pose.orientation.z = rot[2] posestamped.pose.orientation.w = rot[3] self.pub_pose.publish(posestamped) rospy.sleep(0.1) if __name__ == '__main__': rospy.init_node('tf2topic') listener = TransformListener() transformer = TransformerROS() foo = tf2topic() rospy.spin()
else: overriding_robot_base_frame = rospy.get_param('robot_base_frame') if overriding_robot_base_frame != "": calib.transformation.header.frame_id = overriding_robot_base_frame overriding_tracking_base_frame = rospy.get_param('tracking_base_frame') if overriding_tracking_base_frame != "": calib.transformation.child_frame_id = overriding_tracking_base_frame 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()
class Intersection4wayModel(): def __init__(self): # Origin of ther intersection coordinate system is the center of stopline 0. tile_side_length = 0.61 lane_width = 0.205 stopline_thickness = 0.048 center_markings_thickness = 0.025 q = lane_width p = stopline_thickness s = center_markings_thickness self.stopline_poses = [] self.tf1 = TransformerROS() self.tf2 = TransformerROS() def add_stopline_transform(idx, position, angle): orientation = unit_vector( quaternion_from_euler(0, 0, angle, axes='sxyz')) pose = utils.pose(position, orientation) # Add transform for reference stopline poses child_frame = self.stopline_frame(idx) parent_frame = 'intersection' transform = utils.transform_from_pose('intersection', child_frame, pose) self.tf1.setTransform(transform) # Add transform for measured stopline poses child_frame = self.intersection_frame(idx) parent_frame = self.stopline_frame(idx) inverted_pose = utils.invert_pose(utils.pose( position, orientation)) transform = utils.transform_from_pose(parent_frame, child_frame, inverted_pose) self.tf2.setTransform(transform) add_stopline_transform(0, [0.0, 0.0, 0.0], 0) add_stopline_transform( 1, [-(q / 2.0 + s + q + p / 2.0), p / 2.0 + q / 2.0, 0.0], -np.pi / 2.0) add_stopline_transform( 2, [-(q / 2.0 + s + q / 2.0), p / 2.0 + 2 * q + s + p / 2.0, 0.0], -np.pi) add_stopline_transform( 3, [q / 2.0 + p / 2.0, p / 2.0 + q + s + q / 2.0, 0.0], -3.0 * np.pi / 2.0) def stopline_frame(self, idx): return 'stopline_' + str(idx) def intersection_frame(self, idx): return 'intersection_' + str(idx) # axle_pose needs to be in intersection coordinates def get_stopline_poses_reference(self, axle_pose): transform = utils.transform_from_pose('intersection', 'axle', axle_pose) self.tf1.setTransform(transform) stopline_poses = [ self.tf1.transformPose('axle', utils.origin_pose( self.stopline_frame(i))).pose for i in range(4) ] return stopline_poses def get_axle_pose_from_stopline_pose(self, stopline_pose, idx): transform = utils.transform_from_pose('axle', self.stopline_frame(idx), stopline_pose) self.tf2.setTransform(transform) axle_origin = utils.origin_pose('axle') axle_pose = self.tf2.transformPose(self.intersection_frame(idx), axle_origin) return axle_pose
class mapping(): def __init__(self): self.node_name = rospy.get_name() self.tf = TransformListener() self.transformer = TransformerROS() rospy.loginfo("[%s] Initializing " % (self.node_name)) #rospy.Subscriber("/pcl_points", PoseArray, self.call_back, queue_size=1, buff_size = 2**24) sub_pcl = message_filters.Subscriber("/pcl_points", PoseArray) sub_odom = message_filters.Subscriber("/odometry/ground_truth", Odometry) ats = ApproximateTimeSynchronizer((sub_pcl, sub_odom), queue_size=1, slop=0.1) ats.registerCallback(self.call_back) rospy.Subscriber("/move_base_simple/goal", PoseStamped, self.cb_new_goal, queue_size=1) self.pub_map = rospy.Publisher('/local_map', OccupancyGrid, queue_size=1) self.pub_rviz = rospy.Publisher("/wp_line", Marker, queue_size=1) self.pub_poses = rospy.Publisher("/path_points", PoseArray, queue_size=1) self.resolution = 0.25 self.robot = Pose() self.width = 200 self.height = 200 self.origin = Pose() self.local_map = OccupancyGrid() self.dilating_size = 6 self.wall_width = 3 self.start_planning = False self.transpose_matrix = None self.goal = [] self.astar = AStar() self.msg_count = 0 self.planning_range = 20 self.frame_id = "map" def init_param(self): self.occupancygrid = np.zeros((self.height, self.width)) self.local_map = OccupancyGrid() self.local_map.info.resolution = self.resolution self.local_map.info.width = self.width self.local_map.info.height = self.height self.origin.position.x = -self.width * self.resolution / 2. + self.robot.position.x self.origin.position.y = -self.height * self.resolution / 2. + self.robot.position.y self.local_map.info.origin = self.origin def cb_rviz(self, msg): self.click_pt = [msg.pose.position.x, msg.pose.position.y] self.publish_topic() def call_back(self, pcl_msg, odom_msg): self.robot = odom_msg.pose.pose self.msg_count = self.msg_count + 1 self.init_param() self.local_map.header = pcl_msg.header self.local_map.header.frame_id = self.frame_id self.get_tf() if self.transpose_matrix is None: return for i in range(len(pcl_msg.poses)): p = (pcl_msg.poses[i].position.x, pcl_msg.poses[i].position.y, pcl_msg.poses[i].position.z, 1) local_p = np.array(p) global_p = np.dot(self.transpose_matrix, local_p) x, y = self.map2occupancygrid(global_p) width_in_range = (x >= self.width - self.dilating_size or x <= self.dilating_size) height_in_range = (y >= self.height - self.dilating_size or y <= self.dilating_size) if width_in_range or height_in_range: continue # To prevent point cloud range over occupancy grid range self.occupancygrid[y][x] = 100 # map dilating for i in range(self.height): for j in range(self.width): if self.occupancygrid[i][j] == 100: for m in range(-self.dilating_size, self.dilating_size + 1): for n in range(-self.dilating_size, self.dilating_size + 1): if self.occupancygrid[i + m][j + n] != 100: if m > self.wall_width or m < -self.wall_width or n > self.wall_width or n < -self.wall_width: if self.occupancygrid[i + m][j + n] != 90: self.occupancygrid[i + m][j + n] = 50 else: self.occupancygrid[i + m][j + n] = 90 for i in range(self.height): for j in range(self.width): self.local_map.data.append(self.occupancygrid[i][j]) self.pub_map.publish(self.local_map) if self.start_planning: self.path_planning() def get_tf(self): try: position, quaternion = self.tf.lookupTransform( "/map", "/X1/front_laser", rospy.Time(0)) self.transpose_matrix = self.transformer.fromTranslationRotation( position, quaternion) except (LookupException, ConnectivityException, ExtrapolationException): print("Nothing Happen") def path_planning(self): if self.msg_count % 5 != 0: return self.msg_count = 0 cost_map = np.zeros((self.height, self.width)) border = self.planning_range / self.resolution h_min = int((self.height - border) / 2.) h_max = int(self.height - (self.height - border) / 2.) w_min = int((self.height - border) / 2.) w_max = int(self.width - (self.width - border) / 2.) for i in range(self.width): for j in range(self.width): if i > h_min and i < h_max: if j > w_min and j < w_max: cost_map[i][j] = self.occupancygrid[i][j] start_point = self.map2occupancygrid( (self.robot.position.x, self.robot.position.y)) start = (start_point[1], start_point[0]) goal = self.map2occupancygrid(self.goal) end = (goal[1], goal[0]) self.astar.initial(cost_map, start, end) path = self.astar.planning() self.pub_path(path) self.rviz(path) def cb_new_goal(self, p): self.goal = [p.pose.position.x, p.pose.position.y] self.start_planning = True def occupancygrid2map(self, p): x = p[ 0] * self.resolution + self.origin.position.x + self.resolution / 2. y = p[ 1] * self.resolution + self.origin.position.y + self.resolution / 2. return [x, y] def map2occupancygrid(self, p): x = int((p[0] - self.origin.position.x) / self.resolution) y = int((p[1] - self.origin.position.y) / self.resolution) return [x, y] def pub_path(self, path): poses = PoseArray() for i in range(len(path)): p = self.occupancygrid2map([path[i][1], path[i][0]]) pose = Pose() pose.position.x = p[0] pose.position.y = p[1] pose.position.z = 0 poses.poses.append(pose) self.pub_poses.publish(poses) def rviz(self, path): marker = Marker() marker.header.frame_id = self.frame_id marker.type = marker.LINE_STRIP marker.action = marker.ADD marker.scale.x = 0.3 marker.scale.y = 0.3 marker.scale.z = 0.3 marker.color.a = 1.0 marker.color.r = 1.0 marker.color.g = 0. marker.color.b = 0. marker.pose.orientation.x = 0.0 marker.pose.orientation.y = 0.0 marker.pose.orientation.z = 0.0 marker.pose.orientation.w = 1.0 marker.pose.position.x = 0.0 marker.pose.position.y = 0.0 marker.pose.position.z = 0.0 marker.points = [] for i in range(len(path)): p = self.occupancygrid2map([path[i][1], path[i][0]]) point = Point() point.x = p[0] point.y = p[1] point.z = 0 marker.points.append(point) self.pub_rviz.publish(marker)
def __init__(self, *args): TransformerROS.__init__(self, *args)
class particlefilter: def __init__(self, num_particles=100, vis_noise=10, ultra_noise=100, mag_noise=37, linear_noise=.002, angular_noise=2.3, ar_noise=10, ar_resample_rate=10, ar_resample=True): print("Starting a particle filer with %d particles" % num_particles) self.filename = os.path.expanduser("~/Dropbox/thesis_data/" + time.strftime('%Y_%m_%d_%H_%M_%S')) #in the format YYYYMMDDHHMMSS self.fp = open(self.filename + ".txt", "w") self.fp_part = open(self.filename+"_part.txt", "w") self.fp_ar = open(self.filename+"_ar.txt", "w") self.fp_alt = open(self.filename+"_alt.txt", "w") self.fp_sensor = open(self.filename+"_sensor.txt", "w") self.num_particles = num_particles self.vis_noise = vis_noise self.ultra_noise = ultra_noise self.mag_noise = mag_noise self.linear_noise = linear_noise self.angular_noise = angular_noise self.ar_noise = ar_noise self.ar_resample_rate = ar_resample_rate self.ar_resample = ar_resample self.rotY = 0 self.rotX = 0 self.start_mag_heading = 0 self.start_gyr_heading = 0 self.gyr_theta = 0 self.particle_list = [] self.weight_dict = dict() self.first_propagate = True self.first_correct = True self.step = 0 self.est = particle(self) self.acc_est = particle(self) #Estimation using acc and gyr self.vis_est = particle(self) #Estimation using vis odometry and ultrasound self.tag_est = particle(self) #Estimation using visual tags for i in range(num_particles): self.particle_list.append(particle(self)) self.est_pose = Pose() self.est_pub = rospy.Publisher('pf_localization', Pose) self.listener = TransformListener() self.transformer = TransformerROS() self.publish_pose() def propagate_alt(self, delta_t, x_vel, y_vel, altd, rotX, rotY, rotZ): if (self.first_propagate): self.start_gyr_heading = rotZ self.first_propagate = False self.rotX = rotX self.rotY = rotY # print "Zeroed rotZ: %f" % clamp_angle(rotZ - self.start_gyr_heading) delta_theta = clamp_angle((rotZ - self.start_gyr_heading) - self.est.theta) self.vis_est.propagate_alt(delta_t, x_vel, y_vel, altd, delta_theta, False) for particle in self.particle_list: particle.propagate_alt(delta_t, x_vel, y_vel, altd, delta_theta, True) #Move this to correct_alt at some point self.step += 1 self.estimate_equal() self.publish_pose() self.fp_alt.write("%d,%f,%f,%f,%f,%f,%f,%f,%f,%f\n" % (self.step, delta_t, self.est.x, self.est.y, self.est.z, self.est.theta, self.vis_est.x, self.vis_est.y, self.vis_est.z, self.vis_est.theta)) self.fp_sensor.write("%d,%f,%f,%f,%f,%f,%f,%f\n" % (self.step, delta_t, x_vel, y_vel, altd, rotX, rotY, rotZ)) def ar_correct(self, marker): print "Correcting" marker_id = marker.id pose = marker.pose pose_trans = () pose_rot = () #Is this necessary pose_trans = (pose.position.x, pose.position.y, pose.position.z) pose_rot = (pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w) marker_name = "/marker_%d" % marker_id #Get the offset of the marker from the origin try: (marker_trans,marker_rot) = self.listener.lookupTransform('world', marker_name, rospy.Time(0)) except (LookupException, ConnectivityException, ExtrapolationException): print("Unable to find marker transform for marker %d" % marker_id) return try: (base_trans,base_rot) = self.listener.lookupTransform('/ardrone/ardrone_base_link', '/ardrone/ardrone_base_bottomcam', rospy.Time(0)) except (LookupException, ConnectivityException, ExtrapolationException): print("Unable to find ardrone transform") return #Everything is in m not mm pose_mat = numpy.matrix(self.transformer.fromTranslationRotation(pose_trans, pose_rot)) pose_mat_inv = pose_mat.getI() marker_mat = numpy.matrix(self.transformer.fromTranslationRotation(marker_trans, marker_rot)) marker_mat_inv = marker_mat.getI() base_mat = numpy.matrix(self.transformer.fromTranslationRotation(base_trans, base_rot)) base_mat_inv = base_mat.getI() origin = numpy.matrix([[0], [0], [0], [1]]) global_mat = marker_mat * pose_mat_inv * base_mat_inv global_trans = translation_from_matrix(global_mat) global_rot = rotation_from_matrix(global_mat) global_trans = global_trans*1000 #Convert to mm global_heading = clamp_angle(global_rot[0]*180/pi + 180) #Convert to degrees self.tag_est.x = global_trans[0] self.tag_est.y = global_trans[1] self.tag_est.z = global_trans[2] self.tag_est.theta = global_heading if (self.ar_resample): #Weight particles self.weight_particles(self.tag_est.x, self.tag_est.y, self.tag_est.z, self.tag_est.theta, True) #Create new set of particles self.particle_list = [] #Initialize the random selector. Can select items in O(1) time wrand = walkerrandom(self.weight_dict.values(), self.weight_dict.keys()) for i in range(self.num_particles): #Create particle from new value new_particle = particle(self) rand_val = random.random() threshold = self.ar_resample_rate/100.0 if (rand_val < threshold): # print "Particle at (%f, %f, %f, %f)" % (self.tag_est.x, self.tag_est.y, self.tag_est.z, self.tag_est.theta) new_particle = particle(self, self.tag_est.x, self.tag_est.y, self.tag_est.z, self.tag_est.theta) #Randomly pick existing particle else: # print "Picking existing" new_particle = copy(wrand.random()) self.particle_list.append(new_particle) if (len(self.particle_list) != self.num_particles): print("WRONG NUMBER OF PARTICLES") self.fp_ar.write("%d,%d,%f,%f,%f,%f\n" % (self.step, marker_id, self.tag_est.x, self.tag_est.y, self.tag_est.z, self.tag_est.theta)) #Calculate the weight for particles def weight_particles(self, x_est, y_est, z_est, theta_est, ar=False): # print "Weighting......." self.weight_dict = dict() # print "Particle List:" # for particle in self.particle_list: # print particle.to_string() weight_sum = 0 for particle in self.particle_list: #Calculate distances lat_dist = ((x_est - particle.x)**2 + (y_est - particle.y)**2)**.5 vert_dist = abs(z_est - particle.z) theta_dist = abs(theta_est - particle.theta) lat_noise = self.vis_noise vert_noise = self.ultra_noise theta_noise = self.mag_noise #If this module is being called with ar_tag data, use ar_noise if (ar): lat_noise = self.ar_noise vert_noise = self.ar_noise theta_noise = self.ar_noise lat_weight = normpdf(lat_dist, 0, lat_noise) #Potentially do the z distance separately? vert_weight = normpdf(vert_dist, 0, vert_noise) theta_weight = normpdf(theta_dist, 0, theta_noise) weight = lat_weight + vert_weight + theta_weight self.weight_dict[copy(particle)] = weight weight_sum += weight # print "Compare to (%f, %f, %f, %f)" % (x_est, y_est, z_est, theta_est) #Normalize the weights for particle, weight in self.weight_dict.iteritems(): if (weight_sum != 0): self.weight_dict[particle] = weight/weight_sum # print "(%s, %f)" % (particle.to_string(), self.weight_dict[particle]) #Return an estimate of the pose #For now just use linear combination. Should we cluster instead? def estimate(self): self.est = particle(self) total = 0 for part, weight in self.weight_dict.iteritems(): self.est.x += part.x*weight self.est.y += part.y*weight self.est.z += part.z*weight self.est.theta += part.z*weight total += weight self.est.x = self.est.x/total self.est.y = self.est.y/total self.est.z = self.est.z/total self.est.theta = self.est.theta/total def publish_pose(self): self.est_pose.position.x = self.est.x self.est_pose.position.y = self.est.y self.est_pose.position.z = self.est.z rad_theta = self.est.theta*(pi/180) quat = quaternion_about_axis(rad_theta, (0, 0, 1)) self.est_pose.orientation.x = quat[0] self.est_pose.orientation.y = quat[1] self.est_pose.orientation.z = quat[2] self.est_pose.orientation.w = quat[3] self.est_pub.publish(self.est_pose) def estimate_equal(self): self.est = particle(self) x_val = [] y_val = [] z_val = [] theta_val = [] total = 0 for part in self.particle_list: self.est.x += part.x self.est.y += part.y self.est.z += part.z self.est.theta += part.theta x_val.append(part.x) y_val.append(part.y) z_val.append(part.z) theta_val.append(part.theta) total += 1 self.est.x = self.est.x/total self.est.y = self.est.y/total self.est.z = self.est.z/total self.est.theta = self.est.theta/total print "%s : Est" % self.est.to_string() # print self.est.theta def print_particles(self): for particle in self.particle_list: print particle.to_string()
def __init__(self, *args): TransformerROS.__init__(self, *args) thr = TransformListenerThread(self) thr.setDaemon(True) thr.start() self.setUsingDedicatedThread(True)
class map(): def __init__(self): self.node_name = "Map" self.global_map = ObstaclePoseList() self.first = True # process first time self.radius = 1 # to check whether the measurement obstacle is correspond to global map or not self.confi_threshold = 5 # confidence threshold, to determine whether to add point to map or not self.tf = TransformListener() self.transformer = TransformerROS() # Subscribers self.obstacle_list = rospy.Subscriber("/obstacle_list", ObstaclePoseList, self.call_back, queue_size=1) # Publishers self.rviz_pub = rospy.Publisher("/map_viz", Marker, queue_size=1) self.map_list = rospy.Publisher("/map_list", ObstaclePoseList, queue_size=1) def draw(self, ob_list): marker = Marker() marker.header.frame_id = "map" marker.header.stamp = rospy.Time.now() marker.type = marker.SPHERE_LIST marker.action = marker.ADD marker.scale.x = 0.4 marker.scale.y = 0.4 marker.scale.z = 0.4 marker.color.a = 1.0 marker.color.r = 1.0 marker.color.g = 0.0 marker.color.b = 1.0 for i in range(0, ob_list.size): p = Point() p.x = ob_list.list[i].x p.y = ob_list.list[i].y p.z = ob_list.list[i].z marker.points.append(p) self.rviz_pub.publish(marker) def distance(self, a, b): # caculate distance between two 3d points return math.sqrt((a.x - b.x) * (a.x - b.x) + (a.y - b.y) * (a.y - b.y) + (a.z - b.z) * (a.z - b.z)) def update_obs_pos( self, measurement, prior ): # update obstacle position by weighting measurement and original map posterior = ObstaclePose() posterior = prior posterior.x = measurement.x * 0.3 + prior.x * 0.7 posterior.y = measurement.y * 0.3 + prior.y * 0.7 posterior.z = measurement.z * 0.3 + prior.z * 0.7 return posterior def call_back(self, msg): position, quaternion = self.tf.lookupTransform("/utm", "/velodyne", rospy.Time(0)) transpose_matrix = self.transformer.fromTranslationRotation( position, quaternion) print("Process Obstacle List") obs_list = ObstaclePoseList() obs_list = msg for i in range(0, obs_list.size): origin_p = np.array([ obs_list.list[i].x, obs_list.list[i].y, obs_list.list[i].z, 1 ]) new_p = np.dot(transpose_matrix, origin_p) obs_list.list[i].x = new_p[0] obs_list.list[i].y = new_p[1] obs_list.list[i].z = new_p[2] if self.first: # record the first detection to be global map self.global_map = obs_list for i in range(0, self.global_map.size): self.global_map.list[i].confidence = 0 self.first = False for i in range(0, self.global_map.size): self.global_map.list[i].occupy = False for i in range(0, obs_list.size): min_dis = 10000 index = None for j in range(0, self.global_map.size): if not self.global_map.list[ j].occupy: # check whether the point has already been corresponded to another point dis = self.distance(obs_list.list[i], self.global_map.list[j]) if dis < min_dis: index = j min_dis = dis if min_dis < self.radius: # believe that measurement[i] is corresponded to the map[index] self.global_map.list[index].occupy = True self.global_map.list[index] = self.update_obs_pos( obs_list.list[i], self.global_map.list[index]) if self.global_map.list[ index].confidence < self.confi_threshold: self.global_map.list[ index].confidence = self.global_map.list[ index].confidence + 1 else: obs_list.list[i].confidence = 0 self.global_map.list.append(obs_list.list[i]) self.global_map.size = self.global_map.size + 1 map_confi = ObstaclePoseList() map_confi.header = self.global_map.header for i in range(0, self.global_map.size): if self.global_map.list[i].confidence >= self.confi_threshold: map_confi.list.append(self.global_map.list[i]) map_confi.size = map_confi.size + 1 self.draw(map_confi) self.map_list.publish(map_confi) def onShutdown(self): rospy.loginfo("[Global_Map] Shutdown.")
class particlefilter: def __init__(self, num_particles=100, vis_noise=10, ultra_noise=100, mag_noise=37, linear_noise=.002, angular_noise=2.3, ar_noise=10, ar_resample_rate=10, ar_resample=True): print("Starting a particle filer with %d particles" % num_particles) self.filename = os.path.expanduser( "~/Dropbox/thesis_data/" + time.strftime('%Y_%m_%d_%H_%M_%S')) #in the format YYYYMMDDHHMMSS self.fp = open(self.filename + ".txt", "w") self.fp_part = open(self.filename + "_part.txt", "w") self.fp_ar = open(self.filename + "_ar.txt", "w") self.fp_alt = open(self.filename + "_alt.txt", "w") self.fp_sensor = open(self.filename + "_sensor.txt", "w") self.num_particles = num_particles self.vis_noise = vis_noise self.ultra_noise = ultra_noise self.mag_noise = mag_noise self.linear_noise = linear_noise self.angular_noise = angular_noise self.ar_noise = ar_noise self.ar_resample_rate = ar_resample_rate self.ar_resample = ar_resample self.rotY = 0 self.rotX = 0 self.start_mag_heading = 0 self.start_gyr_heading = 0 self.gyr_theta = 0 self.particle_list = [] self.weight_dict = dict() self.first_propagate = True self.first_correct = True self.step = 0 self.est = particle(self) self.acc_est = particle(self) #Estimation using acc and gyr self.vis_est = particle( self) #Estimation using vis odometry and ultrasound self.tag_est = particle(self) #Estimation using visual tags for i in range(num_particles): self.particle_list.append(particle(self)) self.est_pose = Pose() self.est_pub = rospy.Publisher('pf_localization', Pose) self.listener = TransformListener() self.transformer = TransformerROS() self.publish_pose() def propagate_alt(self, delta_t, x_vel, y_vel, altd, rotX, rotY, rotZ): if (self.first_propagate): self.start_gyr_heading = rotZ self.first_propagate = False self.rotX = rotX self.rotY = rotY # print "Zeroed rotZ: %f" % clamp_angle(rotZ - self.start_gyr_heading) delta_theta = clamp_angle((rotZ - self.start_gyr_heading) - self.est.theta) self.vis_est.propagate_alt(delta_t, x_vel, y_vel, altd, delta_theta, False) for particle in self.particle_list: particle.propagate_alt(delta_t, x_vel, y_vel, altd, delta_theta, True) #Move this to correct_alt at some point self.step += 1 self.estimate_equal() self.publish_pose() self.fp_alt.write("%d,%f,%f,%f,%f,%f,%f,%f,%f,%f\n" % (self.step, delta_t, self.est.x, self.est.y, self.est.z, self.est.theta, self.vis_est.x, self.vis_est.y, self.vis_est.z, self.vis_est.theta)) self.fp_sensor.write( "%d,%f,%f,%f,%f,%f,%f,%f\n" % (self.step, delta_t, x_vel, y_vel, altd, rotX, rotY, rotZ)) def ar_correct(self, marker): print "Correcting" marker_id = marker.id pose = marker.pose pose_trans = () pose_rot = () #Is this necessary pose_trans = (pose.position.x, pose.position.y, pose.position.z) pose_rot = (pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w) marker_name = "/marker_%d" % marker_id #Get the offset of the marker from the origin try: (marker_trans, marker_rot) = self.listener.lookupTransform( 'world', marker_name, rospy.Time(0)) except (LookupException, ConnectivityException, ExtrapolationException): print("Unable to find marker transform for marker %d" % marker_id) return try: (base_trans, base_rot) = self.listener.lookupTransform( '/ardrone/ardrone_base_link', '/ardrone/ardrone_base_bottomcam', rospy.Time(0)) except (LookupException, ConnectivityException, ExtrapolationException): print("Unable to find ardrone transform") return #Everything is in m not mm pose_mat = numpy.matrix( self.transformer.fromTranslationRotation(pose_trans, pose_rot)) pose_mat_inv = pose_mat.getI() marker_mat = numpy.matrix( self.transformer.fromTranslationRotation(marker_trans, marker_rot)) marker_mat_inv = marker_mat.getI() base_mat = numpy.matrix( self.transformer.fromTranslationRotation(base_trans, base_rot)) base_mat_inv = base_mat.getI() origin = numpy.matrix([[0], [0], [0], [1]]) global_mat = marker_mat * pose_mat_inv * base_mat_inv global_trans = translation_from_matrix(global_mat) global_rot = rotation_from_matrix(global_mat) global_trans = global_trans * 1000 #Convert to mm global_heading = clamp_angle(global_rot[0] * 180 / pi + 180) #Convert to degrees self.tag_est.x = global_trans[0] self.tag_est.y = global_trans[1] self.tag_est.z = global_trans[2] self.tag_est.theta = global_heading if (self.ar_resample): #Weight particles self.weight_particles(self.tag_est.x, self.tag_est.y, self.tag_est.z, self.tag_est.theta, True) #Create new set of particles self.particle_list = [] #Initialize the random selector. Can select items in O(1) time wrand = walkerrandom(self.weight_dict.values(), self.weight_dict.keys()) for i in range(self.num_particles): #Create particle from new value new_particle = particle(self) rand_val = random.random() threshold = self.ar_resample_rate / 100.0 if (rand_val < threshold): # print "Particle at (%f, %f, %f, %f)" % (self.tag_est.x, self.tag_est.y, self.tag_est.z, self.tag_est.theta) new_particle = particle(self, self.tag_est.x, self.tag_est.y, self.tag_est.z, self.tag_est.theta) #Randomly pick existing particle else: # print "Picking existing" new_particle = copy(wrand.random()) self.particle_list.append(new_particle) if (len(self.particle_list) != self.num_particles): print("WRONG NUMBER OF PARTICLES") self.fp_ar.write("%d,%d,%f,%f,%f,%f\n" % (self.step, marker_id, self.tag_est.x, self.tag_est.y, self.tag_est.z, self.tag_est.theta)) #Calculate the weight for particles def weight_particles(self, x_est, y_est, z_est, theta_est, ar=False): # print "Weighting......." self.weight_dict = dict() # print "Particle List:" # for particle in self.particle_list: # print particle.to_string() weight_sum = 0 for particle in self.particle_list: #Calculate distances lat_dist = ((x_est - particle.x)**2 + (y_est - particle.y)**2)**.5 vert_dist = abs(z_est - particle.z) theta_dist = abs(theta_est - particle.theta) lat_noise = self.vis_noise vert_noise = self.ultra_noise theta_noise = self.mag_noise #If this module is being called with ar_tag data, use ar_noise if (ar): lat_noise = self.ar_noise vert_noise = self.ar_noise theta_noise = self.ar_noise lat_weight = normpdf( lat_dist, 0, lat_noise) #Potentially do the z distance separately? vert_weight = normpdf(vert_dist, 0, vert_noise) theta_weight = normpdf(theta_dist, 0, theta_noise) weight = lat_weight + vert_weight + theta_weight self.weight_dict[copy(particle)] = weight weight_sum += weight # print "Compare to (%f, %f, %f, %f)" % (x_est, y_est, z_est, theta_est) #Normalize the weights for particle, weight in self.weight_dict.iteritems(): if (weight_sum != 0): self.weight_dict[particle] = weight / weight_sum # print "(%s, %f)" % (particle.to_string(), self.weight_dict[particle]) #Return an estimate of the pose #For now just use linear combination. Should we cluster instead? def estimate(self): self.est = particle(self) total = 0 for part, weight in self.weight_dict.iteritems(): self.est.x += part.x * weight self.est.y += part.y * weight self.est.z += part.z * weight self.est.theta += part.z * weight total += weight self.est.x = self.est.x / total self.est.y = self.est.y / total self.est.z = self.est.z / total self.est.theta = self.est.theta / total def publish_pose(self): self.est_pose.position.x = self.est.x self.est_pose.position.y = self.est.y self.est_pose.position.z = self.est.z rad_theta = self.est.theta * (pi / 180) quat = quaternion_about_axis(rad_theta, (0, 0, 1)) self.est_pose.orientation.x = quat[0] self.est_pose.orientation.y = quat[1] self.est_pose.orientation.z = quat[2] self.est_pose.orientation.w = quat[3] self.est_pub.publish(self.est_pose) def estimate_equal(self): self.est = particle(self) x_val = [] y_val = [] z_val = [] theta_val = [] total = 0 for part in self.particle_list: self.est.x += part.x self.est.y += part.y self.est.z += part.z self.est.theta += part.theta x_val.append(part.x) y_val.append(part.y) z_val.append(part.z) theta_val.append(part.theta) total += 1 self.est.x = self.est.x / total self.est.y = self.est.y / total self.est.z = self.est.z / total self.est.theta = self.est.theta / total print "%s : Est" % self.est.to_string() # print self.est.theta def print_particles(self): for particle in self.particle_list: print particle.to_string()
# Compute camera base -> camera_optical transformation opt_base = get_transform(rospy.get_param('camera_optical_frame'), rospy.get_param('camera_base_frame')) # Compute tracking_base_frame -> o2as_ground transformation if not calib.eye_on_hand: grnd_bot = get_transform('o2as_ground', bot) broad = TransformBroadcaster() rate = rospy.Rate(50) baseEst = rospy.get_param('camera_body_frame') try: while not rospy.is_shutdown(): now = rospy.Time.now() broad.sendTransform(trns, rot, now, optEst, bot) # ..., child, parent broad.sendTransform(opt_base[0], opt_base[1], now, baseEst, optEst) rate.sleep() except rospy.ROSInterruptException: transformer = TransformerROS() mat = transformer.fromTranslationRotation(trns, rot) print "\n=== Estimated camera -> robot(effector or base_link) transformation ===" print_mat(mat) if not calib.eye_on_hand: mat = tfs.concatenate_matrices( \ transformer.fromTranslationRotation(*grnd_bot), mat, transformer.fromTranslationRotation(*opt_base)) print "\n=== Estimated camera_base -> ground transformation ===" print_mat(mat)
class LocalTransformer: """This class allows to use the TF class TransformerROS without using the ROS network system or the topic /tf, where transforms are usually published to. Instead a local transformer is saved and allows to publish local transforms, as well the use of TFs convenient lookup functions (see functions below). This class uses the robots (currently only one! supported) URDF file to initialize the tfs for the robot. Moreover, the function update_local_transformer_from_btr updates these tfs by copying the tfs state from the pybullet world.""" def __init__(self, map_frame, odom_frame, projection_namespace, kitchen_namespace): # Frame names of the map and odom frame self.map_frame = map_frame self.odom_frame = odom_frame # Namespaces self.projection_namespace = projection_namespace self.kitchen_namespace = kitchen_namespace # TF tf_stampeds and static_tf_stampeds of the Robot in the BulletWorld: # These are initialized with the function init_transforms_from_urdf and are # used to update the local transformer with the function update_local_transformer_from_btr self.tf_stampeds = [] self.static_tf_stampeds = [] self.local_transformer = TransformerROS(interpolate=True, cache_time=Duration(10.0)) self.init_local_tf_with_tfs_from_urdf() def init_transforms_from_urdf(self): """ This static function gets with the robot name in robot_description.py, the robots URDF file from the "resources" folder in the ROS "pycram" folder. All joints of the URDF are extracted and saved in a list of static and normal TFStamped. :returns: list of static_tf_stampeds, list of tf_stampeds """ # Save joint translations and orientations in tf_stampeds # static_tf_stampeds saves only the static joint translations and orientations self.tf_stampeds = [] self.static_tf_stampeds = [] # Get URDF file path robot_name = robot_description.i.name rospack = rospkg.RosPack() filename = rospack.get_path( 'pycram') + '/resources/' + robot_name + '.urdf' # ... and open it with open(filename) as f: s = f.read() robot = URDF.from_xml_string(s) # Save for every joint in the robot the source_frame, target_frame, # aswell the joints origin, where the translation xyz and rotation rpy # are saved for joint in robot.joints: source_frame = self.projection_namespace + '/' + joint.parent target_frame = self.projection_namespace + '/' + joint.child if joint.origin: if joint.origin.xyz: translation = joint.origin.xyz else: translation = [0, 0, 0] if joint.origin.rpy: roll = joint.origin.rpy[0] pitch = joint.origin.rpy[1] yaw = joint.origin.rpy[2] rotation = transformations.quaternion_from_euler( roll, pitch, yaw) else: rotation = [0, 0, 0, 1] if joint.name in robot_description.i.odom_joints: # since pybullet wont update this joints, these are declared as static translation = [0, 0, 0] rotation = [0, 0, 0, 1] # Wrap the joint attributes in a TFStamped and append it to static_tf_stamped if the joint was fixed tf_stamped = helper.list2tfstamped(source_frame, target_frame, [translation, rotation]) if (joint.type and joint.type == 'fixed' ) or joint.name in robot_description.i.odom_joints: self.static_tf_stampeds.append(tf_stamped) else: self.tf_stampeds.append(tf_stamped) def init_local_tf_with_tfs_from_urdf(self): "This function initializes the local transformer with TFs from the robots URDF." self.init_transforms_from_urdf() for static_tfstamped in self.static_tf_stampeds: self.local_transformer._buffer.set_transform_static( static_tfstamped, "default_authority") for tfstamped in self.tf_stampeds: self.local_transformer.setTransform(tfstamped) def update_robot_state(self): robot = BulletWorld.robot if robot: # First publish (static) joint states to tf # Update tf stampeds which might have changed for tf_stamped in self.tf_stampeds: source_frame = tf_stamped.header.frame_id target_frame = tf_stamped.child_frame_id # Push calculated transformation to the local transformer p, q = robot.get_link_relative_to_other_link( source_frame.replace(self.projection_namespace + "/", ""), target_frame.replace(self.projection_namespace + "/", "")) self.local_transformer.setTransform( helper.list2tfstamped(source_frame, target_frame, [p, q])) # Update the static transforms for static_tf_stamped in self.static_tf_stampeds: self.local_transformer._buffer.set_transform_static( static_tf_stamped, "default_authority") def update_robot_pose(self): robot = BulletWorld.robot if robot: # Then publish pose of robot try: robot_pose = BulletWorld.robot.get_link_position_and_orientation_tf( robot_description.i.base_frame) except KeyError: robot_pose = BulletWorld.robot.get_position_and_orientation() self.publish_robot_pose(helper.ensure_pose(robot_pose)) def update_robot(self): self.update_robot_state() self.update_robot_pose() def update_objects(self): if BulletWorld.current_bullet_world: for obj in list(BulletWorld.current_bullet_world.objects): if obj.name == robot_description.i.name or obj.type == "environment": continue else: published = local_transformer.publish_object_pose( obj.name, obj.get_position_and_orientation()) if not published: logerr("(publisher) Could not publish given pose of %s.", obj.name) def update_static_odom(self): self.publish_pose(self.map_frame, self.projection_namespace + '/' + self.odom_frame, [[0, 0, 0], [0, 0, 0, 1]], static=True) def update_from_btr(self): # Update static odom self.update_static_odom() # Update pose and state of robot self.update_robot() # Update pose of objects which are possibly attached on the robot self.update_objects() ############################################################################################################ ######################################### Query Local Transformer ########################################## ############################################################################################################ def tf_pose_transform(self, source_frame, target_frame, pose, time=None): tf_pose = helper.ensure_pose(pose) tf_time = time if time else self.local_transformer.getLatestCommonTime( source_frame, target_frame) tf_pose_stamped = PoseStamped(Header(0, tf_time, source_frame), tf_pose) return self.tf_pose_stamped_transform(target_frame, tf_pose_stamped) def tf_pose_stamped_transform(self, target_frame, pose_stamped): return helper.pose_stamped2tuple( self.local_transformer.transformPose(target_frame, pose_stamped)) def tf_transform(self, source_frame, target_frame, time=None): tf_time = time if time else self.local_transformer.getLatestCommonTime( source_frame, target_frame) return self.local_transformer.lookupTransform(source_frame, target_frame, tf_time) ############################################################################################################ ####################################### Publish to Local Transformer ###########'########################### ############################################################################################################ def publish_pose(self, frame_id, child_frame_id, pose, static=None): if frame_id != child_frame_id: pose = helper.ensure_pose(pose) tf_stamped = helper.pose2tfstamped(frame_id, child_frame_id, pose) if tf_stamped: if static: self.local_transformer._buffer.set_transform_static( tf_stamped, "default_authority") else: self.local_transformer.setTransform(tf_stamped) return True def publish_object_pose(self, name, pose): """ Publishes the given pose of the object of given name in reference to the map frame to tf. :type name: str :type pose: list or Pose """ if name in robot_description.i.name: return None if name not in robot_description.i.name: pose = helper.ensure_pose(pose) if pose: tf_name = self.projection_namespace + '/' + name if self.projection_namespace else name return self.publish_pose(self.map_frame, tf_name, pose) else: logerr( "(publisher) Could not publish given pose of %s since" " it could not be converted to a Pose object.", name) return None def publish_robot_pose(self, pose): "Publishes the base_frame of the robot in reference to the map frame to tf." robot_pose = helper.ensure_pose(pose) if robot_pose: tf_base_frame = self.projection_namespace + '/' + robot_description.i.base_frame \ if self.projection_namespace \ else robot_description.i.base_frame return self.publish_pose(self.map_frame, tf_base_frame, robot_pose) else: logerr("(publisher) Could not publish given pose of robot since" " it could not be converted to a Pose object.") return None def publish_robots_link_pose(self, link, pose): "Publishes the frame link of the robot in reference to the base_frame of the robot to tf." robot_pose = helper.ensure_pose(pose) if robot_pose: tf_base_frame = self.projection_namespace + '/' + robot_description.i.base_frame \ if self.projection_namespace \ else robot_description.i.base_frame tf_link_frame = self.projection_namespace + '/' + link \ if self.projection_namespace \ else link if not tf_base_frame in tf_link_frame: return self.publish_pose(tf_base_frame, tf_link_frame, robot_pose) else: logerr("(publisher) Could not publish given pose of robot since" " it could not be converted to a Pose object.") return None
# except rospy.ServiceException, e: # print "Service call failed: %s"%e # def usage(): # return "%s [plate_id]"%sys.argv[0] if __name__ == "__main__": rospy.init_node("all") pub_tilt = rospy.Publisher("/teleop/tilt",Float64, queue_size=1) listener = tf.TransformListener() transformer = TransformerROS() rospy.wait_for_service('get_object_pose') get_object_pose = rospy.ServiceProxy('get_object_pose', get_object_pose) object_pose = Pose() try: response = get_object_pose() object_pose = response.pose print(object_pose) except rospy.ServiceException, e: print "Service call failed: %s"%e exit() try: (trans,rot) = listener.lookupTransform('/base_link', "/camera_color_optical_frame", rospy.Time(0)) except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):