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)
