def start_ekf(self, position): q = tf.transformations.quaternion_from_euler(0, 0, position[2]) header = Header(stamp=rospy.Time.now(), frame_id="map") pose = Pose(position=Point(x=position[0], y=position[1], z=0), orientation=Quaternion( x=q[0], y=q[1], z=q[2], w=q[3], )) # Publish pose with covariance stamped. p_c_s = PoseWithCovarianceStamped() p_c = PoseWithCovariance() # These don't matter covariance = np.array([ .01, 0, 0, 0, 0, 0, 0, .01, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, .0001 ])**2 p_c.pose = pose p_c.covariance = covariance p_c_s.header = header p_c_s.header.frame_id = "map" p_c_s.pose = p_c self.set_init_pose(p_c_s)
def generate_pose(self, angle): # Generate a pose, all values but the yaw will be 0. q = transformations.quaternion_from_euler(0, 0, angle) header = Header( stamp=rospy.Time.now(), frame_id="map" ) pose = Pose( position=Point(x=0, y=0, z=0), orientation=Quaternion(x=q[0], y=q[1], z=q[2], w=q[3]) ) # Publish pose stamped - just for displaying in rviz self.pose_est_pub.publish( PoseStamped( header=header, pose=pose ) ) # Publish pose with covariance stamped. p_c_s = PoseWithCovarianceStamped() p_c = PoseWithCovariance() covariance = np.array([1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, .7])**2 p_c.pose = pose p_c.covariance = covariance p_c_s.header = header p_c_s.pose = p_c # Publish pose estimation self.p_c_s_est_pub.publish(p_c_s)
def on_gps(self, msg): if self.localization and (rospy.Time.now() - self.last_correction).to_sec() > 3.0: distance = math.sqrt((self.localization.pose.pose.position.x - msg.pose.pose.position.x)**2.0 + (self.localization.pose.pose.position.y - msg.pose.pose.position.y)**2.0) yaw_localization = euler_from_quaternion( (self.localization.pose.pose.orientation.x, self.localization.pose.pose.orientation.y, self.localization.pose.pose.orientation.z, self.localization.pose.pose.orientation.w))[2] yaw_gps = euler_from_quaternion( (msg.pose.pose.orientation.x, msg.pose.pose.orientation.y, msg.pose.pose.orientation.z, msg.pose.pose.orientation.w))[2] delta_yaw = math.fabs(yaw_localization - yaw_gps) if distance > self.tolerance_xy: # or delta_yaw > self.tolerance_yaw: print(distance, delta_yaw) pose = PoseWithCovarianceStamped() pose.pose = msg.pose pose.header.frame_id = "map" pose.header.stamp = rospy.Time.now() self.initialpose_pub.publish(pose) self.last_correction = rospy.Time.now()
def check_init_pose(self): # ................................................................ # read saved inital pose and publish it ... try: with open(self.saved_pose_uri, 'r') as infile: pose_str = infile.read() oldPose = json_message_converter.convert_json_to_ros_message( 'geometry_msgs/PoseWithCovarianceStamped', pose_str) initPose = PoseWithCovarianceStamped() initPose.pose = oldPose.pose initPose.header.frame_id = oldPose.header.frame_id initPose.header.stamp = rospy.Time.now() rospy.loginfo("[" + rospy.get_name() + "] " + "Initial pose is:\n" + str(initPose)) rospy.sleep(1) while self.initial_pose_topic_pub.get_num_connections() == 0: rospy.logwarn("[" + rospy.get_name() + "] " + "Waiting for subscribers to connect to initpose") rospy.sleep(1) self.initial_pose_topic_pub.publish(initPose) rospy.loginfo("[" + rospy.get_name() + "] " + "Initial pose found and published.") except IOError as e: rospy.logerr("[" + rospy.get_name() + "] " + ": " + str(e))
def joy_callback(self, message): if message.buttons[8] == 1 and message.buttons[10] == 1: self.sync_signal_count += 1 else: self.sync_signal_count = 0 if self.sync_signal_count > MIN_SYNC_MESSAGES and time.time() - self.last_sync_time > MIN_DELTA_T_BETWEEN_SYNCS: self.last_sync_time = time.time() if self.update_idx == 0: self.update_idx += 1 rospy.loginfo('Received first sync (origin)') return if self.relevant_update_index != self.update_idx: rospy.loginfo('Identified joystick press related to irrelevant update index #%d ==> ignoring' % self.update_idx) self.update_idx += 1 return pose_key = self.ugv_poses.keys()[self.update_idx] pose = self.ugv_poses[pose_key] dx = (pose[0] - self.init_pose[0]) * self.resolution # TODO: order??? dy = (pose[1] - self.init_pose[1]) * self.resolution alpha = (np.pi/2 - self.init_yaw) * (-1) x = dx * np.cos(alpha) - dy * np.sin(alpha) # TODO: scale, check alpha, check x and y on image (might need to switch x and y...) y = dx * np.sin(alpha) + dy * np.cos(alpha) odom_message = self.odom_message pose_message = PoseWithCovarianceStamped() pose_message.header.stamp = odom_message.header.stamp pose_message.header.frame_id = odom_message.header.frame_id pose_message.pose = odom_message.pose pose_message.pose.pose.position.x = x pose_message.pose.pose.position.y = y rospy.loginfo('Performing global update from image %s to pose (%f, %f)' % (pose_key, x, y)) self.update_idx += 1 self.pose_pub.publish(pose_message)
def plan_next_wp(self): if not self.done(): self.curr_target = self.waypoints[self.waypoint_index] print "Heading towards:" print self.curr_target start_msg = PoseWithCovarianceStamped() start_msg.header.frame_id = '/map' start_msg.header.stamp = rospy.Time.now() start_msg.pose = PoseWithCovariance() start_msg.pose.pose = Pose() start_msg.pose.pose.position.x = self.start_pose[0] start_msg.pose.pose.position.y = self.start_pose[1] start_msg.pose.pose.orientation = self.start_pose[2] self.start_pub.publish(start_msg) target_msg = PoseStamped() target_msg.header.frame_id = '/map' target_msg.header.stamp = rospy.Time.now() target_msg.pose = Pose() target_msg.pose.position.x = self.curr_target[0] target_msg.pose.position.y = self.curr_target[1] target_msg.pose.orientation = Utils.angle_to_quaternion( self.curr_target[2]) self.target_pub.publish(target_msg)
def __init__(self): _, map_info = Utils.get_map(MAP_TOPIC) self.curr_target = None self.start_pose = None self.waypoint_index = 0 self.waypoints = np.array([[2600, map_info.height - 660, 0], [1880, map_info.height - 440, 0], [1435, map_info.height - 545, 0], [1250, map_info.height - 460, 0], [540, map_info.height - 835, 0]], dtype='float64') self.start = np.array([[2500, map_info.height - 640, 0]], dtype='float64') Utils.map_to_world(self.waypoints, map_info) Utils.map_to_world(self.start, map_info) self.start = self.start[0, :] self.start[2] = np.deg2rad(-15) for i in range(1, len(self.waypoints)): theta = np.arctan2(self.waypoints[i, 1] - self.waypoints[i - 1, 1], self.waypoints[i, 0] - self.waypoints[i - 1, 0]) self.waypoints[i - 1, 2] = theta self.waypoints[0, 2] = np.deg2rad(-15) self.waypoints[1, 2] = np.deg2rad(180) self.waypoints[2, 2] = np.deg2rad(135) self.waypoints[3, 2] = np.deg2rad(170) self.waypoints[4, 2] = np.deg2rad(-135) self.pose_cb(rospy.wait_for_message(LOCALIZATION_TOPIC, PoseStamped)) self.pose_sub = rospy.Subscriber(LOCALIZATION_TOPIC, PoseStamped, self.pose_cb) self.start_pub = rospy.Publisher(START_TOPIC, PoseWithCovarianceStamped, queue_size=10) self.target_pub = rospy.Publisher(TARGET_TOPIC, PoseStamped, queue_size=10) self.target_reached_sub = rospy.Subscriber(CONTROLLER_DONE_TOPIC, PoseStamped, self.complete_wp_cb) rospy.sleep(1) start_msg = PoseWithCovarianceStamped() start_msg.header.frame_id = '/map' start_msg.header.stamp = rospy.Time.now() start_msg.pose = PoseWithCovariance() start_msg.pose.pose = Pose() start_msg.pose.pose.position.x = self.start[0] start_msg.pose.pose.position.y = self.start[1] start_msg.pose.pose.orientation = Utils.angle_to_quaternion( self.start[2]) self.start_pub.publish(start_msg) rospy.sleep(2) self.plan_next_wp()
def set_start(self,location): # rospy.loginfo("got to set_start") zero_twist = Twist() gazebo_state = ModelState() zero_twist.linear.x = 0.0 zero_twist.linear.x = 0.0 zero_twist.linear.z = 0.0 zero_twist.angular.x = 0.0 zero_twist.angular.y = 0.0 zero_twist.angular.z = 0.0 gazebo_state.model_name = self.robot gazebo_state.pose = self._gazebo_pose(location) gazebo_state.twist = zero_twist gazebo_state.reference_frame = self.g_frame amcl_initial_pose = PoseWithCovarianceStamped() amcl_initial_pose.pose = self._amcl_pose(location) amcl_initial_pose.header.frame_id = self.a_frame amcl_initial_pose.header.stamp = rospy.Time.now() self.set_gazebo_state(gazebo_state) rospy.sleep(1) attempts = 0 while attempts < 10: self.publisher.publish(amcl_initial_pose) rospy.sleep(1) if self.localized(amcl_initial_pose.pose.pose): return 0 return 1
def callback(data): header = data.header pose_w_c = data.pose msg = PoseWithCovarianceStamped() msg.header = header msg.pose = pose_w_c pub.publish(msg)
def execute(self, userdata): p = PoseWithCovarianceStamped() p.pose = userdata.pose_current rospy.loginfo(userdata.pose_current) self.initialpose_pub = rospy.Publisher('/initialpose', PoseWithCovarianceStamped) self.initialpose_pub.publish(p) return 'succeeded'
def compose_msg(self): header = Header() header.stamp = rospy.Time.now() header.frame_id = 'map' point_msg = Point(self.robot_x[-1], self.robot_y[-1], 0) # use most recent pos with no z-coord orientation_quat = R.from_euler( 'xyz', [0, 0, self.robot_theta[-1] ]).as_quat() # pitch is rotation about z-axis in euler angles pose_cov = np.diag([0.05, 0.05, 0, 0, 0, 0.01]).flatten() quat_msg = Quaternion(orientation_quat[0], orientation_quat[1], orientation_quat[2], orientation_quat[3]) pose_with_cov = PoseWithCovariance() pose_with_cov.pose = Pose(point_msg, quat_msg) pose_with_cov.covariance = pose_cov stamped_msg = PoseWithCovarianceStamped() stamped_msg.header = header stamped_msg.pose = pose_with_cov try: pub = rospy.Publisher('uwb_nodes', PoseWithCovarianceStamped, queue_size=1) pub.publish(stamped_msg) except rospy.ROSInterruptException as e: print(e.getMessage()) pass
def poseRvizCallback(self, msg): """ same as above """ ps_msg = PoseWithCovarianceStamped() ps_msg.header = msg.header ps_msg.pose = msg.pose.pose self.poseCallback(ps_msg)
def amcl_pose(self, data): pub = rospy.Publisher('amcl_pose', PoseWithCovarianceStamped, queue_size=1) robot_pose = PoseWithCovarianceStamped() robot_pose.header = data.header robot_pose.pose = data.pose.pose pub.publish(robot_pose)
def odom_callback(msg): global pose_publisher msg_ = PoseWithCovarianceStamped() msg_.header.stamp = rospy.Time.now() msg_.pose = msg.pose msg_.header.frame_id = "odom" pose_publisher.publish(msg_) time.sleep(3)
def publish_sensors(self, msg): """ publishes noisy position values for each robot """ pwc = PoseWithCovariance() auv = "akon" # X Meas if rospy.get_param("kalman/measurements/x_noise_type") == "normal": rospy.loginfo_once("Normal Error on X") sigma = rospy.get_param("kalman/measurements/x_sigma") pwc.pose.position.x = np.random.normal( self.auvs[auv][ODOM_INDEX].pose.pose.position.x, sigma) pwc.covariance[0] = sigma ** 2 else: rospy.loginfo_once("Uniform Error on X") sigma = rospy.get_param("kalman/measurements/x_sigma") dist_range = np.sqrt(12) * sigma low = self.auvs[auv][ODOM_INDEX].pose.pose.position.x - dist_range / 2 high = self.auvs[auv][ODOM_INDEX].pose.pose.position.x + dist_range / 2 pwc.pose.position.x = np.random.uniform(low, high) pwc.covariance[0] = sigma ** 2 # Y Meas if rospy.get_param("kalman/measurements/y_noise_type") == "normal": rospy.loginfo_once("Normal Error on Y") sigma = rospy.get_param("kalman/measurements/y_sigma") pwc.pose.position.y = np.random.normal( self.auvs[auv][ODOM_INDEX].pose.pose.position.y, sigma) pwc.covariance[7] = sigma ** 2 else: rospy.loginfo_once("Uniform Error on Y") sigma = rospy.get_param("kalman/measurements/y_sigma") dist_range = np.sqrt(12) * sigma low = self.auvs[auv][ODOM_INDEX].pose.pose.position.x - dist_range / 2 high = self.auvs[auv][ODOM_INDEX].pose.pose.position.x + dist_range / 2 pwc.pose.position.y = np.random.uniform(low, high) pwc.covariance[7] = sigma ** 2 # Z Meas if rospy.get_param("kalman/measurements/z_noise_type") == "normal": rospy.loginfo_once("Normal Error on Z") sigma = rospy.get_param("kalman/measurements/z_sigma") pwc.pose.position.z = np.random.normal( self.auvs[auv][ODOM_INDEX].pose.pose.position.z, sigma) pwc.covariance[14] = sigma ** 2 else: rospy.loginfo_once("Uniform Error on Z") sigma = rospy.get_param("kalman/measurements/z_sigma") dist_range = np.sqrt(12) * sigma low = self.auvs[auv][ODOM_INDEX].pose.pose.position.x - dist_range / 2 high = self.auvs[auv][ODOM_INDEX].pose.pose.position.x + dist_range / 2 pwc.pose.position.z = np.random.uniform(low, high) pwc.covariance[14] = sigma ** 2 pwc.pose.orientation = self.auvs[auv][ODOM_INDEX].pose.pose.orientation pwcs = PoseWithCovarianceStamped() pwcs.header.seq = self.seq pwcs.header.stamp = rospy.Time.now() pwcs.header.frame_id = "base_link" pwcs.pose = pwc self.pose_sensor_pub.publish(pwcs) self.seq += 1
def execute(self, userdata): rospy.sleep(3) rospy.logwarn("New position!!") p = PoseWithCovarianceStamped() p.pose = userdata.pose_current rospy.loginfo(userdata.pose_current) self.initialpose_pub = rospy.Publisher('/initialpose', PoseWithCovarianceStamped) self.initialpose_pub.publish(p) return 'succeeded'
def pose_cb(data): global path path.header = data.header pose = PoseWithCovarianceStamped() pose.header = data.header pose.pose = data.pose.pose #path_file.write(str(pose.pose.position.x) + ',' + str(pose.pose.position.y) + '\n') path.poses.append(pose) path_pub.publish(path)
def strToPoseWithCovariance(str): x, y, z, t = translate_module_position(str) msg = PoseWithCovarianceStamped() msg.header = Header() msg.header.stamp.secs = int(t) msg.header.stamp.nsecs = int((t % 1) * 1000000000.0) msg.pose = PoseWithCovariance() msg.pose.pose = Pose() msg.pose.pose.position = Point(x, y, z) return msg
def coordinatesToPoseWithCovariance(coordinates): x = float(coordinates[0]) y = float(coordinates[1]) z = float(coordinates[2]) msg = PoseWithCovarianceStamped() msg.header = Header() msg.header.stamp = rospy.Time.now() msg.pose = PoseWithCovariance() msg.pose.pose = Pose() msg.pose.pose.position = Point(x, y, z) return msg
def load_gazebo_models(blockPose=Pose(position=Point(x=-3.5, y=1.5, z=0)), blockRefFrame="map", targetPose=Pose(position=Point(x=-2.5, y=-3.5, z=0)), targetRefFrame="map"): #Publish to /initialpose topic, so that rviz and gazebo match up initPosePub = rospy.Publisher('initialpose', PoseWithCovarianceStamped, queue_size=10) initPoseMsg = PoseWithCovarianceStamped() initPoseMsg.header = Header() initPoseMsg.pose = PoseWithCovariance() initPoseMsg.header.seq = 0 initPoseMsg.header.stamp = rospy.Time.now() initPoseMsg.header.frame_id = "robot_description" initPoseMsg.pose.pose = Pose() initPoseMsg.pose.covariance = [0.25, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.25, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.06853891945200942] initPoseMsg.pose.pose.position = Point() initPoseMsg.pose.pose.orientation = Quaternion() # TODO: change to inputted robot starting positions later initPoseMsg.pose.pose.position.x = -4.0 initPoseMsg.pose.pose.position.y = 4.0 initPoseMsg.pose.pose.position.z = 0 initPoseMsg.pose.pose.orientation.x = 0 initPoseMsg.pose.pose.orientation.y = 0 initPoseMsg.pose.pose.orientation.z = 0 initPoseMsg.pose.pose.orientation.w = 0 #rate = rospy.Rate(1) initPosePub.publish(initPoseMsg) # Get models' paths model_path = rospkg.RosPack().get_path('starter')+"/models/" # Load Block SDF block_xml = '' with open (model_path + "cyl_ob/model.sdf", "r") as block_file: block_xml=block_file.read().replace('\n', '') # Load Target SDF #target_xml = '' #with open (model_path + "target/model.sdf", "r") as target_file: # target_xml=target_file.read().replace('\n', '') # Spawn Block SDF rospy.wait_for_service('/gazebo/spawn_sdf_model') try: spawn_block_sdf = rospy.ServiceProxy('/gazebo/spawn_sdf_model', SpawnModel) block_resp_sdf = spawn_block_sdf("cyl_block", block_xml, "/", blockPose, blockRefFrame) except rospy.ServiceException, e: rospy.logerr("Spawn SDF service call failed: {0}".format(e))
def state_callback(msg): stateMsg = RobotState() stateMsg.twist.twist = msg.twist.twist stateMsg.twist.header = msg.header stateMsg.pose.pose = msg.pose.pose stateMsg.pose.header = msg.header statePub.publish(stateMsg) poseMsg = PoseWithCovarianceStamped() poseMsg.header = msg.header poseMsg.pose = msg.pose posePub.publish(poseMsg)
def pos_callback(pos_msg): odom = pos_msg pos1 = PoseWithCovarianceStamped() pos1.header = odom.header pos1.pose = odom.pose pos1.pose # pos = PoseStamped() # pos.header = odom.header # pos.pose = odom.pose.pose pub_pos_cov.publish(pos1)
def realodomCB(data): global posecovarpub, posepub posecov_msg = PoseWithCovarianceStamped() pose_msg = PoseStamped() data.pose.pose.position.x = data.pose.pose.position.x - t_prior_visited[0] data.pose.pose.position.y = data.pose.pose.position.y - t_prior_visited[1] data.pose.pose.position.z = data.pose.pose.position.z - t_prior_visited[2] posecov_msg.pose = data.pose pose_msg.pose = data.pose.pose posecov_msg.header = data.header pose_msg.header = data.header posepub.publish(pose_msg) posecovarpub.publish(posecov_msg)
def publish_pose(self,pose,time): #Generate pose q = tf.transformations.quaternion_from_euler(0, 0, pose[2]) header = Header( stamp=rospy.Time.now(), frame_id="map" ) pose = Pose( position=Point( x=pose[0], y=pose[1], z=0 ), orientation=Quaternion( x=q[0], y=q[1], z=q[2], w=q[3], ) ) # Publish pose stamped self.pose_est_pub.publish( PoseStamped( header=header, pose=pose ) ) # Publish pose with covariance stamped. p_c_s = PoseWithCovarianceStamped() p_c = PoseWithCovariance() # These don't matter covariance = np.array([ 1, 0, 0, 0, 0, 0, 0,.2*self.std_err, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, self.std_err])**2 p_c.pose = pose p_c.covariance = covariance p_c_s.header = header p_c_s.header.frame_id = "map" p_c_s.pose = p_c # if time.time() - self.start_time < 5: # self.p_c_s_init_pub.publish(p_c_s) # else: self.p_c_s_est_pub.publish(p_c_s)
def utm_cb(msgIn): global wp_num, deg2rad # recieve waypoint converted to odom rospy.loginfo("converted waypoint recieved.") posCovStamped = PoseWithCovarianceStamped() posCovStamped.pose = msgIn.pose posCovStamped.header = msgIn.header # add in des orientation q = quaternion_from_euler(0, 0, gps_waypoints[wp_num][3] * deg2rad) posCovStamped.pose.pose.orientation.x = q[0] posCovStamped.pose.pose.orientation.y = q[1] posCovStamped.pose.pose.orientation.z = q[2] posCovStamped.pose.pose.orientation.w = q[3] # append to global waypoint array waypoints.append(posCovStamped)
def _push_mocap_pose(self,data): if self.tf.frameExists(self.frame) and self.tf.frameExists("/world"): t = self.tf.getLatestCommonTime(self.frame, "/world") position, quaternion = self.tf.lookupTransform("/world", self.frame, t) msg = PoseWithCovarianceStamped() pose = PoseWithCovariance() pose.pose.position.x = position[0] pose.pose.position.y = position[1] pose.pose.position.z = position[2] pose.pose.orientation.x = quaternion[0] pose.pose.orientation.y = quaternion[1] pose.pose.orientation.z = quaternion[2] pose.pose.orientation.w = quaternion[3] pose.covariance = [0] * 36 #could tune the covariance matrices? pose.covariance[0] = -1 msg.pose = pose msg.header.stamp = rospy.Time.now() msg.header.frame_id = "/bug/base_link" self.pose_converted.publish(msg)
def publish_gps_point(self, latitude, longitude, height, variance, status, pub_navsatfix, pub_pose, pub_point, pub_transform): # Navsatfix message. navsatfix_msg = NavSatFix() navsatfix_msg.header.stamp = rospy.Time.now() navsatfix_msg.header.frame_id = self.navsatfix_frame_id navsatfix_msg.position_covariance_type = NavSatFix.COVARIANCE_TYPE_APPROXIMATED navsatfix_msg.status.service = NavSatStatus.SERVICE_GPS navsatfix_msg.latitude = latitude navsatfix_msg.longitude = longitude navsatfix_msg.altitude = height navsatfix_msg.status.status = status navsatfix_msg.position_covariance = [ variance[0], 0, 0, 0, variance[1], 0, 0, 0, variance[2] ] # Local Enu coordinate. (east, north, up) = self.geodetic_to_enu(latitude, longitude, height) # Pose message. pose_msg = PoseWithCovarianceStamped() pose_msg.header.stamp = navsatfix_msg.header.stamp pose_msg.header.frame_id = self.enu_frame_id pose_msg.pose = self.enu_to_pose_msg(east, north, up, variance) # Point message. point_msg = PointStamped() point_msg.header.stamp = navsatfix_msg.header.stamp point_msg.header.frame_id = self.enu_frame_id point_msg.point = self.enu_to_point_msg(east, north, up) # Transform message. transform_msg = TransformStamped() transform_msg.header.stamp = navsatfix_msg.header.stamp transform_msg.header.frame_id = self.enu_frame_id transform_msg.child_frame_id = self.transform_child_frame_id transform_msg.transform = self.enu_to_transform_msg(east, north, up) # Publish. pub_navsatfix.publish(navsatfix_msg) pub_pose.publish(pose_msg) pub_point.publish(point_msg) pub_transform.publish(transform_msg)
def callbackPose(poseCV): covariance = [] for i in range(0, 6): covariance.append([]) for j in range(0, 6): covariance[i].append(poseCV.pose.covariance[6 * i + j]) covariance = np.array(covariance) covariance = np.linalg.pinv(covariance) msg = PoseWithCovarianceStamped() msg.header = Header(poseCV.header.seq, poseCV.header.stamp, poseCV.header.frame_id) msg.pose = poseCV.pose data = [] for i in covariance: for j in i: data.append(float(j)) msg.pose.covariance = data ppub.publish(msg) print("(x,y,yaw): (" + str(covariance[0, 0]) + ", " + str(covariance[1, 1]) + ", " + str(covariance[5, 5]) + ")")
def _push_mocap_pose(self, data): if self.tf.frameExists(self.frame) and self.tf.frameExists("/world"): t = self.tf.getLatestCommonTime(self.frame, "/world") position, quaternion = self.tf.lookupTransform( "/world", self.frame, t) msg = PoseWithCovarianceStamped() pose = PoseWithCovariance() pose.pose.position.x = position[0] pose.pose.position.y = position[1] pose.pose.position.z = position[2] pose.pose.orientation.x = quaternion[0] pose.pose.orientation.y = quaternion[1] pose.pose.orientation.z = quaternion[2] pose.pose.orientation.w = quaternion[3] pose.covariance = [0] * 36 #could tune the covariance matrices? pose.covariance[0] = -1 msg.pose = pose msg.header.stamp = rospy.Time.now() msg.header.frame_id = "/bug/base_link" self.pose_converted.publish(msg)
def poseMask(pose1): global pose2 global goal2 global backTarget pose2 = PoseCv() pose2.header.frame_id = 'map' pose2.pose = pose1.pose #pubPD.publish(pose2) p = np.array([pose1.pose.pose.position.x, pose1.pose.pose.position.y]) g = np.array([goal1.pose.position.x, goal1.pose.position.y]) d = np.linalg.norm(p-g) if d < 10: theta = 2 * np.arcsin(goal1.pose.orientation.z) goal2.pose.position.x = goal1.pose.position.x #+ dist * np.cos(theta) goal2.pose.position.y = goal1.pose.position.y #+ dist * np.sin(theta) else: theta = 2 * np.arcsin(goal1.pose.orientation.z) goal2.pose.position.x = goal1.pose.position.x - dist * np.cos(theta) goal2.pose.position.y = goal1.pose.position.y - dist * np.sin(theta)
def spencer_human_tracking_callback(self, msg): with self.data_lock: # after each callback, reset closests self.closest_human_pose = None self.closest_human_twist = None min_dist = np.inf min_i = -1 # TODO: Maybe it's better if we low pass filter these tracks to keep # just humans that have been detected for a minimum period of time #rospy.loginfo("...........................................................") for i, trk_person in enumerate(msg.tracks): # Create the stamped object human_pose = PoseWithCovarianceStamped() # msg contains a header with the frame id for all poses human_pose.header = msg.header human_pose.pose = trk_person.pose # from the list of tracked persons, find the closest ... (dist, human_pose_glob) = self.getDistToHuman(human_pose) #rospy.loginfo("ID: "+str(i)+" Dist: "+str(dist) +" Pose:\n"+str(human_pose)) if dist < min_dist: min_i = i min_dist = dist self.closest_human_pose = human_pose_glob self.closest_human_twist = self.transformTwistWC(trk_person.twist, msg.header, self.global_frame_id) (xh0, yh0, hh0, vh0, wh0) = self.getHumanState() rospy.logdebug_throttle(1, "Node [" + rospy.get_name() + "] " + "Closest human at: Pose ( " + str(xh0) + ", " + str(yh0) + ", " + str(hh0*180.0/np.pi) + " deg), " + "Speed ( " + str(vh0) + " m/sec, " + str(wh0*180.0/np.pi) + " deg/sec) " ) #rospy.loginfo("...........................................................") if (min_dist>0): self.publishQTCdata()
def send_pose(self, point): # send pose for initing the localization message = PoseWithCovarianceStamped() message.header = self.make_header() pose = PoseWithCovariance() pose.pose = Pose() pose.pose.position = Point() pose.pose.position.x = point[0] pose.pose.position.y = point[1] pose.pose.position.z = 0 pose.pose.orientation = Quaternion() pose.pose.orientation.x = 0 pose.pose.orientation.y = 0 pose.pose.orientation.z = 0 pose.pose.orientation.w = 1 pose.covariance = [ 0.25, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.25, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.06853891945200942 ] message.pose = pose self.pose_pub.publish(message)
def publish_pose(self,pose,stamp): #Generate pose q = tf.transformations.quaternion_from_euler(0, 0, pose[2]) header = Header( stamp=stamp, frame_id="map" ) pose = Pose( position=Point(x=pose[0], y=pose[1], z=0), orientation=Quaternion( x=q[0], y=q[1], z=q[2], w=q[3]) ) # Publish pose stamped self.pose_est_pub.publish( PoseStamped( header=header, pose=pose ) ) self.br.sendTransform((self.pose_est[0], self.pose_est[1], .125), q, rospy.Time.now(), "base_link", "map") # Publish pose with covariance stamped. p_c_s = PoseWithCovarianceStamped() p_c = PoseWithCovariance() covariance = np.array([.05, 0, 0, 0, 0, 0, 0, .05, 0, 0, 0, 0, 0, 0, .05, 0, 0, 0, 0, 0, 0, .05, 0, 0, 0, 0, 0, 0, .05, 0, 0, 0, 0, 0, 0, .05])**2 p_c.pose = pose p_c.covariance = covariance p_c_s.header = header p_c_s.pose = p_c self.p_c_s_est_pub.publish(p_c_s)
def callback(data): global gohome, home, done_init if home is None: home = data.pose.pose if not gohome: pub = rospy.Publisher('/initialpose', PoseWithCovarianceStamped, queue_size=1) #rospy.loginfo("Setting Pose") p = PoseWithCovarianceStamped() msg = PoseWithCovariance() #print data.pose.pose msg.pose = data.pose.pose #Pose(Point(2.64534568787, 4.98263931274, 0.000), Quaternion(0.000, 0.000, -0.990151822567, 0.139997750521)) msg.covariance = [ 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ] p.pose = msg pub.publish(p)
def setestimate(): pub = rospy.Publisher('initialpose', PoseWithCovarianceStamped, queue_size=1) p = PoseWithCovarianceStamped() msg = PoseWithCovariance() msg.pose = Pose(Point(0, 0, 0.190), Quaternion(0.000, 0.000, 0.223, 0.975)) msg.covariance = [ 0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0 ] p.pose = msg rate = rospy.Rate(10) index = 0 while index < 10: # This is hacky hacky Rik 24-11-2015 index += 1 pub.publish(p) rate.sleep()
def start_ekf(self, position): q = tf.transformations.quaternion_from_euler(0, 0, position[2]) header = Header( stamp = rospy.Time.now(), frame_id = "map" ) pose = Pose( position = Point( x = position[0], y = position[1], z = 0 ), orientation = Quaternion( x = q[0], y = q[1], z = q[2], w = q[3], ) ) # Publish pose with covariance stamped. p_c_s = PoseWithCovarianceStamped() p_c = PoseWithCovariance() # These don't matter covariance = np.array([.01, 0, 0, 0, 0, 0, 0, .01, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, .0001]) ** 2 p_c.pose = pose p_c.covariance = covariance p_c_s.header = header p_c_s.header.frame_id = "map" p_c_s.pose = p_c self.set_init_pose(p_c_s)