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 publishPose(): 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, 1.0 ] x = 0 y = 0 z = 0 qx = 0 qy = 0 qz = 0 qw = 1 time_now = rospy.Time.now() id_ = '/map' poseWCS = PoseWithCovarianceStamped() poseWCS.header.stamp = time_now poseWCS.header.frame_id = id_ poseWCS.pose.pose = Pose(Point(x, y, z), Quaternion(qx, qy, qz, qw)) poseWCS.pose.covariance = covariance_ initialpose_poseWCS_publisher.publish(poseWCS) poseWC = PoseWithCovariance() poseWC.pose = Pose(Point(x, y, z), Quaternion(qx, qy, qz, qw)) poseWC.covariance = covariance_ twistWC = TwistWithCovariance() twistWC.twist = Twist(Vector3(0.0, 0.0, 0.0), Vector3(0.0, 0.0, 0.0)) twistWC.covariance = covariance_ odom = Odometry() odom.header.stamp = time_now odom.header.frame_id = id_ odom.pose = poseWC odom.twist = twistWC fakelocalisation_poseWCS_publisher.publish(odom)
def _amcl_pose(self,location): pose = PoseWithCovariance() # print(self.a_locations[location]) pose.pose.position = message_converter.convert_dictionary_to_ros_message('geometry_msgs/Point',self.a_locations[location]['position']) pose.pose.orientation = message_converter.convert_dictionary_to_ros_message('geometry_msgs/Quaternion',self.a_locations[location]['orientation']) pose.covariance = self.a_locations[location]['covariance'] return pose
def create_and_append_submap_constraint_msg(self, candidate_a, candidate_b, T_L_a_L_b, submap_msg): submap_msg.id_from.append(candidate_a.id) submap_msg.timestamp_from.append(candidate_a.get_pivot_timestamp_ros()) submap_msg.robot_name_from.append(candidate_a.robot_name) submap_msg.id_to.append(candidate_b.id) submap_msg.timestamp_to.append(candidate_b.get_pivot_timestamp_ros()) submap_msg.robot_name_to.append(candidate_b.robot_name) t = T_L_a_L_b[0:3,3] q = Rotation.from_matrix(T_L_a_L_b[0:3,0:3]).as_quat() # x, y, z, w pose_cov_msg = PoseWithCovariance() pose_msg = Pose() pose_msg.position.x = t[0] pose_msg.position.y = t[1] pose_msg.position.z = t[2] 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_cov_msg.pose = pose_msg submap_msg.T_a_b.append(pose_cov_msg) return submap_msg
def state_cb(self, msg): if rospy.Time.now( ) - self.last_state_sub_time < self.state_sub_max_prd: return self.last_state_sub_time = rospy.Time.now() if self.target in msg.name: target_index = msg.name.index(self.target) twist = msg.twist[target_index] enu_pose = navigator_tools.pose_to_numpy(msg.pose[target_index]) self.last_ecef = self.enu_to_ecef(enu_pose[0]) self.last_enu = enu_pose self.last_odom = Odometry( header=navigator_tools.make_header(frame='/enu'), child_frame_id='/measurement', pose=PoseWithCovariance( pose=navigator_tools.numpy_quat_pair_to_pose( *self.last_enu)), twist=TwistWithCovariance(twist=twist)) self.last_absodom = Odometry( header=navigator_tools.make_header(frame='/ecef'), child_frame_id='/measurement', pose=PoseWithCovariance( pose=navigator_tools.numpy_quat_pair_to_pose( self.last_ecef, self.last_enu[1])), twist=TwistWithCovariance(twist=twist))
def publish_odom(self, *args): if self.last_odom is None or self.position_offset is None: return msg = self.last_odom if self.target in msg.name: header = mil_ros_tools.make_header(frame='/map') target_index = msg.name.index(self.target) twist = msg.twist[target_index] # Add position offset to make the start position (0, 0, -depth) position_np, orientation_np = mil_ros_tools.pose_to_numpy( msg.pose[target_index]) pose = mil_ros_tools.numpy_quat_pair_to_pose( position_np - self.position_offset, orientation_np) self.state_pub.publish(header=header, child_frame_id='/base_link', pose=PoseWithCovariance(pose=pose), twist=TwistWithCovariance(twist=twist)) header = mil_ros_tools.make_header(frame='/world') twist = msg.twist[target_index] pose = msg.pose[target_index] self.world_state_pub.publish( header=header, child_frame_id='/base_link', pose=PoseWithCovariance(pose=pose), twist=TwistWithCovariance(twist=twist)) else: # fail return
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 state_cb(self, msg): if self.target in msg.name: target_index = msg.name.index(self.target) twist = msg.twist[target_index] enu_pose = navigator_tools.pose_to_numpy(msg.pose[target_index]) self.last_ecef = self.enu_to_ecef(enu_pose[0]) self.last_enu = enu_pose self.last_odom = Odometry( header=navigator_tools.make_header(frame='/enu'), child_frame_id='/base_link', pose=PoseWithCovariance( pose=navigator_tools.numpy_quat_pair_to_pose( *self.last_enu)), twist=TwistWithCovariance(twist=twist)) self.last_absodom = Odometry( header=navigator_tools.make_header(frame='/ecef'), child_frame_id='/base_link', pose=PoseWithCovariance( pose=navigator_tools.numpy_quat_pair_to_pose( self.last_ecef, self.last_enu[1])), twist=TwistWithCovariance(twist=twist))
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 test_world_model_and_recognizer_prediction(self): world_model_reset = rospy.ServiceProxy( '/env/asr_world_model/empty_found_object_list', Empty) rp_ism_node_reset = rospy.ServiceProxy('/rp_ism_node/reset', Empty) world_model_reset() rp_ism_node_reset() world_model_reset = rospy.ServiceProxy( '/env/asr_world_model/empty_found_object_list', Empty) world_model_reset() req = PushFoundObjectRequest() detected_pbd_object = AsrObject() detected_pbd_object.type = 'CoffeeBox' detected_pbd_object.header.frame_id = '/map' detected_pbd_object.identifier = '' detected_pbd_object.providedBy = 'textured' detected_pbd_object.colorName = 'textured' detected_pbd_object.meshResourcePath = 'package://asr_object_database/rsc/databases/textured_objects/CoffeeBox/CoffeeBox.dae' detected_pose = Pose() detected_pose.position.x = 1 detected_pose.position.y = 2 detected_pose.position.z = 3 detected_pose.orientation.w = 0.695641823146944 detected_pose.orientation.x = -0.702895791692416 detected_pose.orientation.y = -0.102411625776331 detected_pose.orientation.z = 0.107386306462868 detected_pose_with_covariance = PoseWithCovariance() detected_pose_with_covariance.pose = detected_pose detected_pbd_object.sampledPoses.append(detected_pose_with_covariance) push_found_object = rospy.ServiceProxy( '/env/asr_world_model/push_found_object', PushFoundObject) push_found_object(detected_pbd_object) get_found_objects = rospy.ServiceProxy( '/env/asr_world_model/get_found_object_list', GetFoundObjectList) resp = get_found_objects() self.assertEqual(len(resp.object_list), 1) for found_object in resp.object_list: self.assertEqual(len(found_object.sampledPoses), 1) find_scenes = rospy.ServiceProxy('/rp_ism_node/find_scenes', FindScenes) find_scenes_req = FindScenesRequest() for pbd_object in resp.object_list: find_scenes_req.objects.append(pbd_object) find_scenes_resp = find_scenes(find_scenes_req) self.assertGreater(find_scenes_resp.result_size, 0) rp_ism_node_predict = rospy.ServiceProxy( '/rp_ism_node/get_point_cloud', GetPointCloud) pose_prediction_resp = rp_ism_node_predict() self.assertGreater(len(pose_prediction_resp.point_cloud.elements), 500)
def callback(self, data): lat = data.latitude lon = data.longitude alt = data.altitude e, n, self.zone_number, self.zone_letter = utm.from_latlon(lat, lon) if not self.start_pos_was_set: self.start_e = e self.start_n = n """ if self.offset: self.start_e -= self.offset.translation.x self.start_n -= self.offset.translation.y """ if self.offset: self.start_e -= 0.4 self.start_n -= 0.4 self.start_pos_was_set = True self.start_alt = data.altitude position = Point() position.x = e - self.start_e position.y = n - self.start_n position.z = 0.0 erel = e - self.start_e # relative position to start position in meters nrel = n - self.start_n alt_rel = alt - self.start_alt # empty as the gps data doesnt include orientation orientation = Quaternion() orientation.w = 1.0 mypose = Pose() mypose.orientation = orientation mypose.position = position if self.fix_covariance: covariance = np.array([1.5, 0, 0, 0, 1.5, 0, 0, 0, 3]) else: covariance = data.position_covariance covariance = np.reshape(covariance, (3, 3)) cov6x6 = np.zeros((6, 6)) cov6x6[:3, :3] = covariance cov6x6_flat = np.reshape(cov6x6, (36, )).astype(np.float64) pose_with_cov = PoseWithCovariance() pose_with_cov.covariance = cov6x6_flat pose_with_cov.pose = mypose odom = Odometry() odom.pose = pose_with_cov odom.header = data.header odom.header.stamp = data.header.stamp odom.header.stamp = rospy.Time.now() odom.header.frame_id = "map" # odom.twist stays empty as gps doesn't have this data self.publisher.publish(odom)
def pub_xy(self): msg = PoseWithCovariance() msg.pose.position.x = self.ballloc_xyz[0] msg.pose.position.y = self.ballloc_xyz[1] msg.pose.position.z = self.ballloc_xyz[2] msg.covariance[0] = self.error msg.covariance[8] = self.error msg.covariance[15] = self.error # msg.covariance[15] = 0.0 self.pos_pub.publish(msg)
def __init__(self): super(SpeedCalculator, self).__init__() rospy.Subscriber('/odom', Odometry, self.onOdom) rospy.Subscriber('/cmd_vel', Twist, self.onCmd) self.pose = PoseWithCovariance() self.last_pose = PoseWithCovariance() self.cmd_vel = Twist() self.cmd_vel_t = rospy.Time.now() self.cmd_vels = [] self.mes_vels = []
def publish_sensors(self, msg): """ publishes noisy position values for each robot """ pwc = PoseWithCovariance() twc = TwistWithCovariance() auv = "akon" # X Meas rospy.loginfo_once("Normal Error on X") sigma = rospy.get_param("kalman/measurements/x_sigma") dot_sigma = rospy.get_param("kalman/measurements/x_dot_sigma") pwc.pose.position.x = np.random.normal( self.auvs[auv][ODOM_INDEX].pose.pose.position.x, sigma) pwc.covariance[0] = sigma**2 twc.twist.linear.x = np.random.normal( self.auvs[auv][ODOM_INDEX].twist.twist.linear.x, dot_sigma) twc.covariance[0] = dot_sigma**2 # Y Meas rospy.loginfo_once("Normal Error on Y") sigma = rospy.get_param("kalman/measurements/y_sigma") dot_sigma = rospy.get_param("kalman/measurements/y_dot_sigma") pwc.pose.position.y = np.random.normal( self.auvs[auv][ODOM_INDEX].pose.pose.position.y, sigma) pwc.covariance[7] = sigma**2 twc.twist.linear.y = np.random.normal( self.auvs[auv][ODOM_INDEX].twist.twist.linear.y, dot_sigma) twc.covariance[7] = dot_sigma**2 # Z Meas rospy.loginfo_once("Normal Error on Z") sigma = rospy.get_param("kalman/measurements/z_sigma") dot_sigma = rospy.get_param("kalman/measurements/z_dot_sigma") pwc.pose.position.z = np.random.normal( self.auvs[auv][ODOM_INDEX].pose.pose.position.z, sigma) pwc.covariance[14] = sigma**2 twc.twist.linear.z = np.random.normal( self.auvs[auv][ODOM_INDEX].twist.twist.linear.z, dot_sigma) twc.covariance[14] = dot_sigma**2 pwc.pose.orientation = self.auvs[auv][ODOM_INDEX].pose.pose.orientation odom_msg = Odometry() odom_msg.header.seq = self.seq odom_msg.header.stamp = rospy.Time.now() odom_msg.header.frame_id = "base_link" odom_msg.child_frame_id = odom_msg.header.frame_id odom_msg.pose = pwc odom_msg.twist = twc self.odom_sensor_pub.publish(odom_msg) self.seq += 1
def late_measurements_period(self): self.output_dir = f'{get_data_dir(os.environ["CODEDIR"])}/test_dir/{get_filename_without_extension(__file__)}' os.makedirs(self.output_dir, exist_ok=True) self._ros_process = None model = BebopModel() filter = KalmanFilter(model=model) measurement_rate = 5 prediction_rate = 15 data = {'predicted': [], 'adjusted': [], 'measured': []} # send measurement 0 t = 0 measurement = Odometry( header=Header(stamp=to_ros_time(t)), pose=PoseWithCovariance(pose=Pose(position=Point(z=0.))), twist=TwistStamped(twist=Twist(linear=Point(z=0.)))) result = filter.kalman_correction(measurement, 1. / prediction_rate) print_result(result, 'measurement: ') data['measured'].append((t, 0)) data['adjusted'].append((t, result.pose.pose.position.x)) # run for 3 seconds for _ in range(1, 3 * prediction_rate): t = _ * 1 / prediction_rate # predict control command = TwistStamped(Header(stamp=to_ros_time(t)), Twist(linear=Point(x=1.0))) result = filter.kalman_prediction(command, 1. / prediction_rate) print_result(result, 'prediction: ') data['predicted'].append( (filter.tnext, result.pose.pose.position.x)) if _ % measurement_rate == 0: # predict measurement measurement = Odometry( header=Header(stamp=to_ros_time(t - 0.01)), pose=PoseWithCovariance(pose=Pose(position=Point( x=0.1 * (_ // measurement_rate)))), twist=TwistStamped(twist=Twist(linear=Point( x=0.5 * (_ // measurement_rate))))) result = filter.kalman_correction(measurement, 1. / prediction_rate) print_result(result, 'measurement: ') data['measured'].append((t, 0.1 * (_ // measurement_rate))) data['adjusted'].append( (filter.tmeas, result.pose.pose.position.x)) return data
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 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 pub_odom(img): pose_covar = PoseWithCovariance(Pose(Point(0, 0, 0), Quaternion()), None) odom = Odometry(Header(frame_id='odom'), 'base_link', pose_covar, None) xy = detector.calculate_best_position(img) #for bln in detector.balloon_positions: #print bln # Don't publish a pose if location can't be reliably determined if xy is None: print("No location found") return yaw_angle = detector.calculate_angle() # publish odometry message header = odom.header #header.seq = data.header.seq #header.stamp = data.header.stamp pose = odom.pose.pose pos = pose.position pos.x, pos.y = xy quaternion = pose.orientation quaternion.z, quaternion.w = math.sin(yaw_angle / 2), math.cos(yaw_angle / 2) odom_pub.publish(odom)
def publish_estimates(self, timestamp, nav_estimate): ne = NetworkEstimate() for asset in self.asset2id.keys(): if "surface" in asset: continue # else: # print("publishing " + asset + "'s estimate") # Construct Odometry Msg for Asset nav_covpt = np.array(nav_estimate.pose.covariance).reshape(6, 6) nav_covtw = np.array(nav_estimate.twist.covariance).reshape(6, 6) mean, cov = self.filter.get_asset_estimate(asset) pose = Pose(Point(mean[0],mean[1],mean[2]), \ nav_estimate.pose.pose.orientation) pose_cov = np.zeros((6, 6)) pose_cov[:3, :3] = cov[:3, :3] pose_cov[3:, 3:] = nav_covpt[3:, 3:] pwc = PoseWithCovariance(pose, list(pose_cov.flatten())) tw = Twist(Vector3(mean[3], mean[4], mean[5]), nav_estimate.twist.twist.angular) twist_cov = np.zeros((6, 6)) twist_cov[:3, :3] = cov[3:6, 3:6] twist_cov[3:, 3:] = nav_covtw[3:, 3:] twc = TwistWithCovariance(tw, list(twist_cov.flatten())) h = Header(self.update_seq, timestamp, "map") o = Odometry(h, "map", pwc, twc) ae = AssetEstimate(o, asset) ne.assets.append(ae) self.asset_pub_dict[asset].publish(o) self.network_pub.publish(ne)
def get_lines(self, fmt): t = Time() t.secs = 0 # MESSAGE_TO_TUM_SHORT line = ROSMsg2CSVLine.to(fmt, Point(), t, ROSMessageTypes.GEOMETRY_MSGS_VECTOR3) line = ROSMsg2CSVLine.to(fmt, PointStamped(), t, ROSMessageTypes.GEOMETRY_MSGS_POINTSTAMPED) line = ROSMsg2CSVLine.to(fmt, Vector3(), t, ROSMessageTypes.GEOMETRY_MSGS_VECTOR3) line = ROSMsg2CSVLine.to(fmt, Vector3Stamped(), t, ROSMessageTypes.GEOMETRY_MSGS_VECTOR3STAMPED) line = ROSMsg2CSVLine.to( fmt, PoseWithCovarianceStamped(), t, ROSMessageTypes.GEOMETRY_MSGS_POSEWITHCOVARIANCESTAMPED) line = ROSMsg2CSVLine.to( fmt, PoseWithCovariance(), t, ROSMessageTypes.GEOMETRY_MSGS_POSEWITHCOVARIANCE) line = ROSMsg2CSVLine.to(fmt, PoseStamped(), t, ROSMessageTypes.GEOMETRY_MSGS_POSESTAMPED) line = ROSMsg2CSVLine.to(fmt, Pose(), t, ROSMessageTypes.GEOMETRY_MSGS_POSE) line = ROSMsg2CSVLine.to(fmt, Quaternion(), t, ROSMessageTypes.GEOMETRY_MSGS_QUATERNION) line = ROSMsg2CSVLine.to( fmt, QuaternionStamped(), t, ROSMessageTypes.GEOMETRY_MSGS_QUATERNIONSTAMPED) line = ROSMsg2CSVLine.to(fmt, Transform(), t, ROSMessageTypes.GEOMETRY_MSGS_TRANSFORM) line = ROSMsg2CSVLine.to( fmt, TransformStamped(), t, ROSMessageTypes.GEOMETRY_MSGS_TRANSFORMSTAMPED) return line
def GetOdomFromState(state, spot_wrapper, use_vision=True): """Maps odometry data from robot state proto to ROS Odometry message Args: data: Robot State proto spot_wrapper: A SpotWrapper object Returns: Odometry message """ odom_msg = Odometry() local_time = spot_wrapper.robotToLocalTime( state.kinematic_state.acquisition_timestamp) odom_msg.header.stamp = rospy.Time(local_time.seconds, local_time.nanos) if use_vision == True: odom_msg.header.frame_id = 'vision' tform_body = get_vision_tform_body( state.kinematic_state.transforms_snapshot) else: odom_msg.header.frame_id = 'odom' tform_body = get_odom_tform_body( state.kinematic_state.transforms_snapshot) odom_msg.child_frame_id = 'body' pose_odom_msg = PoseWithCovariance() pose_odom_msg.pose.position.x = tform_body.position.x pose_odom_msg.pose.position.y = tform_body.position.y pose_odom_msg.pose.position.z = tform_body.position.z pose_odom_msg.pose.orientation.x = tform_body.rotation.x pose_odom_msg.pose.orientation.y = tform_body.rotation.y pose_odom_msg.pose.orientation.z = tform_body.rotation.z pose_odom_msg.pose.orientation.w = tform_body.rotation.w odom_msg.pose = pose_odom_msg twist_odom_msg = GetOdomTwistFromState(state, spot_wrapper).twist odom_msg.twist = twist_odom_msg return odom_msg
def set_initial_position(self): """ Provides initial pose guess and waits until it is corroborated @author Callum """ self.log('Setting pose') while self.pos is None: self.sleep() pose = PoseWithCovarianceStamped( header=get_header(), pose=PoseWithCovariance(pose=self.pos.to_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.06853892326654787 ])) tries = 0 # Wait until self.amcl_pos == position while self.amcl_pos is None or self.amcl_pos.distance_from( self.pos) > 0.5: tries += 1 self.initialpose_pub.publish(pose) self.rate_limiter.sleep() self.log('Set pose to {} after {} tries'.format(self.pos, tries), LOG_SUCCESS)
def Predict(self, dt, xVel, yawVel): dt = 0.004 # Fix dt to avoid computation issues # State-Transition Matrix A_t = np.array([[ 1.0, 0.0, 0.0, cos(self.X_t[2]) * dt, 0.0, cos(self.X_t[2]) * (dt**2) / 2 ], [ 0.0, 1.0, 0.0, sin(self.X_t[2]) * dt, 0.0, sin(self.X_t[2]) * (dt**2) / 2 ], [0.0, 0.0, 1.0, 0.0, dt, 0.0], [0.0, 0.0, 0.0, 1.0, 0.0, dt], [0.0, 0.0, 0.0, 0.0, 1.0, 0.0], [0.0, 0.0, 0.0, 0.0, 0.0, 1.0]]) # Check control difference u_t = np.array([ 0.0, 0.0, 0.0, (xVel - self.X_t[3]), (yawVel - self.X_t[4]), -self.X_t[5] ]) # To ensure Zero Acceleration behaviour # Ground truth data self.X_t = A_t @ self.X_t + u_t self.length = self.length + xVel * dt if (self.test and self.ros): # Send the Update of the Ground Truth to Ros header = Header() header.stamp = rospy.Time.now( ) # Note you need to call rospy.init_node() before this will work header.frame_id = "odom" # since all odometry is 6DOF we'll need a quaternion created from yaw odom_quat = tf.transformations.quaternion_from_euler( 0, 0, self.X_t[2]) # next, we'll publish the pose message over ROS pose = Pose(Point(self.X_t[0], self.X_t[1], 0.), Quaternion(*odom_quat)) pose_covariance = [0] * 36 pose_t = PoseWithCovariance(pose, pose_covariance) # next, we'll publish the twist message over ROS twist = Twist(Vector3(self.X_t[3], 0, self.X_t[5]), Vector3(0.0, 0.0, self.X_t[4])) twist_covariance = [0] * 36 twist_t = TwistWithCovariance(twist, twist_covariance) odom_t = Odometry(header, "base_link", pose_t, twist_t) # publish the message self.odom_t_pub.publish(odom_t)
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 __update_odometry(self, linear_offset, angular_offset, tf_delay): self.__heading = (self.__heading + angular_offset) % 360 q = tf.transformations.quaternion_from_euler( 0, 0, math.radians(self.__heading)) self.__pose.position.x += math.cos( math.radians(self.__heading )) * self.__distance_per_cycle * self.__twist.linear.x self.__pose.position.y += math.sin( math.radians(self.__heading )) * self.__distance_per_cycle * self.__twist.linear.x self.__pose.position.z = 0.33 self.__pose.orientation = Quaternion(q[0], q[1], q[2], q[3]) now = rospy.Time.now() + rospy.Duration(tf_delay) self.__tfb.sendTransform( (self.__pose.position.x, self.__pose.position.y, self.__pose.position.z), q, now, 'base_link', 'odom') o = Odometry() o.header.stamp = now o.header.frame_id = 'odom' o.child_frame_id = 'base_link' o.pose = PoseWithCovariance(self.__pose, None) o.twist = TwistWithCovariance() o.twist.twist.linear.x = self.__distance_per_second * self.__twist.linear.x o.twist.twist.angular.z = math.radians( self.__rotation_per_second) * self.__twist.angular.z
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, **kwargs): assert all(['_' + key in self.__slots__ for key in kwargs.keys()]), \ "Invalid arguments passed to constructor: %r" % kwargs.keys() from std_msgs.msg import Header self.header = kwargs.get('header', Header()) from geometry_msgs.msg import PoseWithCovariance self.pose = kwargs.get('pose', PoseWithCovariance())
def update_pose(self, data): # transform to map coordinates self.tf_listener.waitForTransform("/map", "/base_footprint", rospy.Time(), rospy.Duration(3.0)) (translation, rotation) = self.tf_listener.lookupTransform('/map', '/base_footprint', rospy.Time(0)) self.x = translation[0] self.y = translation[1] euler = tf.transformations.euler_from_quaternion(rotation) self.th = euler[2] # t = self.tf_listener.getLatestCommonTime("/map","/odom") # pose_to_transform = PoseStamped() # pose_to_transform.header = data.header # pose_to_transform.pose = data.pose.pose # pose_in_map = self.tf_listener.transformPose("/map", pose_to_transform) # # unpack # self.x = pose_in_map.pose.position.x # self.y = pose_in_map.pose.position.y # self.th = pose_in_map.pose.orientation.w self.pose = PoseWithCovariance() self.pose.pose.position.x = self.x self.pose.pose.position.y = self.y # self.pose.pose.orientation.z = self.th self.pose.pose.orientation = data.pose.pose.orientation self.write_dict_to_topic()
def publish_maxavg(distribution): sorted_dist = sorted(distribution[:], key=lambda x: x['dist']) print sorted_dist[:1] avg_pose = {'x': 0, 'y': 0} selected_ones = sorted_dist[:1] for measurement in selected_ones: avg_pose['x'] += measurement['x'] * measurement['dist'] avg_pose['y'] += measurement['y'] * measurement['dist'] avg_pose['x'] /= sum(item['dist'] for item in selected_ones) avg_pose['y'] /= sum(item['dist'] for item in selected_ones) pose = Pose(position=Point(x=avg_pose['x'], y=avg_pose['y']), orientation=Quaternion(x=0.707, y=0, z=0, w=0.707)) posewithcovar = PoseWithCovarianceStamped( header=Header(stamp=rospy.Time.now(), frame_id='map'), pose=PoseWithCovariance(pose=pose, covariance=np.zeros((6, 6)).flatten())) pub.publish(posewithcovar) pub_array.publish( PoseArray(header=Header(stamp=rospy.Time.now(), frame_id='map'), poses=[ Pose(position=Point(x=p['x'], y=p['y']), orientation=Quaternion(x=0.707, y=0, z=0, w=0.707)) for p in selected_ones ]))
def Control(self, cmd_vel): now = rospy.get_time() dt = now - self.control_t self.control_t = now z_x_dot = cmd_vel.linear.x z_yaw_dot = cmd_vel.angular.z z_x_dot_cov = 0.001 z_yaw_dot_cov = 0.01 """ # Make sure the execution is safe self.lock.acquire() try: self.Z = np.append(self.Z, np.array([z_x_dot, z_yaw_dot])) self.R = np.append(self.R, np.array([z_x_dot_cov,z_yaw_dot_cov])) self.H = np.column_stack([self.H, np.array([0,0,0,1,0,0]), np.array([0,0,0,0,1,0])]) self.J_H = np.column_stack([self.J_H, np.array([0,0,0,1,0,0]), np.array([0,0,0,0,1,0])]) self.control_measure = True finally: self.lock.release() # release self.lock, no matter what """ self.control_state = np.array([z_x_dot, z_yaw_dot]) #print("Control " + str(self.control_state)) # Send the Update to Ros header = Header() header.stamp = rospy.Time.now( ) # Note you need to call rospy.init_node() before this will work header.frame_id = "odom" # since all odometry is 6DOF we'll need a quaternion created from yaw odom_quat = tf.transformations.quaternion_from_euler(0, 0, self.X_t[2]) # next, we'll publish the pose message over ROS pose = Pose(Point(self.X_t[0], self.X_t[1], 0.), Quaternion(*odom_quat)) pose_covariance = [0] * 36 pose_control = PoseWithCovariance(pose, pose_covariance) twist_covariance = [0] * 36 twist_covariance[0] = z_x_dot_cov twist_covariance[35] = z_yaw_dot_cov twist_control = TwistWithCovariance(cmd_vel, twist_covariance) odom_control = Odometry(header, "base_link", pose_control, twist_control) # publish the message self.odom_control_pub.publish(odom_control)
def __init__(self, **kwargs): assert all('_' + key in self.__slots__ for key in kwargs.keys()), \ 'Invalid arguments passed to constructor: %s' % \ ', '.join(sorted(k for k in kwargs.keys() if '_' + k not in self.__slots__)) from std_msgs.msg import Header self.header = kwargs.get('header', Header()) from geometry_msgs.msg import PoseWithCovariance self.pose = kwargs.get('pose', PoseWithCovariance())
def get_new_pose_with_covariance(self): from geometry_msgs.msg import PoseWithCovarianceStamped from geometry_msgs.msg import PoseWithCovariance from geometry_msgs.msg import Pose from geometry_msgs.msg import Point from geometry_msgs.msg import Quaternion return PoseWithCovarianceStamped(None, PoseWithCovariance(Pose(Point(1.23, 4.56, 7.89), Quaternion(0,0,0,1)), identity6x6))
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_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 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)