def state2pose(self, state, cov):
#print(cov)
output = PoseWithCovarianceStamped()
output.pose.pose.position.x = state[0]
output.pose.pose.position.y = state[1]
output.pose.pose.position.z = 0
oppo = tf.transformations.quaternion_from_euler(0,0,state[2])
output.pose.pose.orientation.x = oppo[0]
output.pose.pose.orientation.y = oppo[1]
output.pose.pose.orientation.z = oppo[2]
output.pose.pose.orientation.w = oppo[3]
new_cov = self.robot_whole_cov
#print(new_cov)
new_cov[0] = cov[0]
new_cov[1] = cov[1]
new_cov[5] = cov[2]
new_cov[6] = cov[3]
new_cov[7] = cov[4]
new_cov[11] = cov[5]
new_cov[30] = cov[6]
new_cov[31] = cov[7]
new_cov[35] = cov[8]
output.pose.covariance = new_cov.flatten()
h = std_msgs.msg.Header()
h.stamp = rospy.Time.now()
h.frame_id = 'base_link'
output.header = h
return output
def make_pose_covariance_stamped_msg(t,R,Cov):
"""
Returns a pose stamped message from a translation vector and rotation matrix (4x4) for publishing.
NOTE: Does not set the target frame.
"""
pose_cov_stamped_msg = PoseWithCovarianceStamped()
#
pose_cov_stamped_msg.header = Header()
pose_cov_stamped_msg.header.stamp = rospy.Time.now()
#
pose_msg = Pose()
pose_msg.position.x = t[0]
pose_msg.position.y = t[1]
pose_msg.position.z = t[2]
#
quat = quaternion_from_matrix(R)
pose_msg.orientation.x = quat[0]
pose_msg.orientation.y = quat[1]
pose_msg.orientation.z = quat[2]
pose_msg.orientation.w = quat[3]
#
pose_cov_stamped_msg.pose.pose = pose_msg
pose_cov_stamped_msg.pose.covariance = Cov
return pose_cov_stamped_msg
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 get_data_body(self, body_id):
try:
resp1 = self.call_body_data(body_id)
except rospy.ServiceException as exc:
print("Service did not process request: " + str(exc))
msg = PoseWithCovarianceStamped()
pose = Pose()
pose.position.x = resp1.pos.x
pose.position.y = resp1.pos.y
pose.position.z = resp1.pos.z
quaternion = tf.transformations.quaternion_from_euler(resp1.pos.roll, resp1.pos.pitch, resp1.pos.yaw)
#type(pose) = geometry_msgs.msg.Pose
pose.orientation.x = quaternion[0]
pose.orientation.y = quaternion[1]
pose.orientation.z = quaternion[2]
pose.orientation.w = quaternion[3]
h = std_msgs.msg.Header()
h.stamp = rospy.Time.now()
msg.header = h
msg.header.frame_id= 'world'
msg.pose.covariance = self.mocap_covariance
msg.pose.pose = pose
return msg
def default(self, ci='unused'):
if 'valid' not in self.data or self.data['valid']:
pose = PoseWithCovarianceStamped()
pose.header = self.get_ros_header()
pose.pose.pose = get_pose(self)
if 'covariance_matrix' in self.data:
pose.pose.covariance = self.data['covariance_matrix']
self.publish(pose)
def publishInitialPose(xPos, yPos, angle):
pub = rospy.Publisher("/initialpose", PoseWithCovarianceStamped)
start = PoseWithCovarianceStamped()
start.header = g.header
start.pose.pose.position.x = xPos
start.pose.pose.position.y = yPos
start.pose.pose.orientation = g.quat
pub.publish(start)
def default(self, ci="unused"):
if "valid" not in self.data or self.data["valid"]:
pose = PoseWithCovarianceStamped()
pose.header = self.get_ros_header()
pose.pose.pose = get_pose(self)
if "covariance_matrix" in self.data:
pose.pose.covariance = self.data["covariance_matrix"]
self.publish(pose)
def _init_nav(self, pose):
pub = rospy.Publisher('/initialpose', PoseWithCovarianceStamped, queue_size=1)
rospy.sleep(1.0)
initialPose = PoseWithCovarianceStamped()
initialPose.header = pose.header
initialPose.pose.pose = pose.pose
p_cov = np.array([0.0]*36)
initialPose.pose.covariance = tuple(p_cov.ravel().tolist())
pub.publish(initialPose)
def state2pose(self, state, cov):
robot_output = PoseWithCovarianceStamped()
robot_output.pose.pose.position.x = state[0]
robot_output.pose.pose.position.y = state[1]
robot_cov = np.zeros((36,))
robot_cov[0] = cov[0,0]
robot_cov[1] = cov[0,1]
robot_cov[5] = cov[0,2]
robot_cov[6] = cov[1,0]
robot_cov[7] = cov[1,1]
robot_cov[11] = cov[1,2]
robot_cov[30] = cov[2,0]
robot_cov[31] = cov[2,1]
robot_cov[35] = cov[2,2]
robot_output.pose.covariance = robot_cov.flatten()
oppo = tf.transformations.quaternion_from_euler(0,0,state[2])
robot_output.pose.pose.orientation.x = oppo[0]
robot_output.pose.pose.orientation.y = oppo[1]
robot_output.pose.pose.orientation.z = oppo[2]
robot_output.pose.pose.orientation.w = oppo[3]
target_output = PoseWithCovarianceStamped()
target_output.pose.pose.position.x = state[3]
target_output.pose.pose.position.y = state[4]
target_cov = np.zeros((36,))
target_cov[0] = cov[3,3]
target_cov[1] = cov[3,4]
target_cov[6] = cov[4,3]
target_cov[7] = cov[4,4]
target_output.pose.covariance = target_cov.flatten()
h = std_msgs.msg.Header()
h.stamp = rospy.Time.now()
h.frame_id = 'odom'
robot_output.header = h
target_output.header = h
return target_output, robot_output
def reset_pose(self):
"""Sets the current pose to START. Doesn't move the robot."""
loginfo("Resetting pose.")
req = PoseWithCovarianceStamped()
req.header = Header(stamp=Time.now(), frame_id='/map')
req.pose.pose = self._x_y_yaw_to_pose(START_X, START_Y, START_YAW)
req.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.06853891945200942, 0.0, 0.0,
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.initial_pose_pub.publish(req)
self.go_to_start()
def state2pose_sim(self, state, cov):
robot_output = PoseWithCovarianceStamped()
robot_output.pose.pose.position.x = state[0]
robot_output.pose.pose.position.y = state[1]
robot_cov = np.zeros((36, ))
robot_cov[0] = cov[0, 0]
robot_cov[1] = cov[0, 1]
#robot_cov[5] = cov[0,2]
robot_cov[6] = cov[1, 0]
robot_cov[7] = cov[1, 1]
#robot_cov[11] = cov[1,2]
#robot_cov[30] = cov[2,0]
#robot_cov[31] = cov[2,1]
#robot_cov[35] = cov[2,2]
robot_output.pose.covariance = robot_cov.flatten()
oppo = tf.transformations.quaternion_from_euler(0, 0, self.yaw)
robot_output.pose.pose.orientation.x = oppo[0]
robot_output.pose.pose.orientation.y = oppo[1]
robot_output.pose.pose.orientation.z = oppo[2]
robot_output.pose.pose.orientation.w = oppo[3]
target_output = PoseWithCovarianceStamped()
target_output.pose.pose.position.x = state[2]
target_output.pose.pose.position.y = state[3]
target_cov = np.zeros((36, ))
target_cov[0] = cov[2, 2]
target_cov[1] = cov[2, 3]
target_cov[6] = cov[3, 2]
target_cov[7] = cov[3, 3]
target_output.pose.covariance = target_cov.flatten()
h = std_msgs.msg.Header()
h.stamp = self.t #rospy.Time.now()
h.frame_id = 'odom'
robot_output.header = h
target_output.header = h
return target_output, robot_output
def spin(self):
rate = rospy.Rate(self.update_freq)
while not rospy.is_shutdown():
A = np.diag([1, 1, 1])
u = [0, 0, 0]
self.kf.predict(A, u)
#if self.measurement_msg:
# valid_marker = self.update(self.msg_to_measurement(self.measurement_msg))
# if valid_marker: self.valid_detection_pub.publish(valid_marker)
self.last_transform.header.stamp = rospy.Time.now()
self.broadcaster.sendTransform(self.last_transform)
# determine u by checking if the drone is in motion
"""
if False and self.cf1_vel:
K_vel = 0.5
vx = self.cf1_vel.twist.linear.x * K_vel
vy = self.cf1_vel.twist.linear.y * K_vel
w = self.cf1_vel.twist.angular.z * K_vel
dt = 1.0/self.update_freq
A = [1 + abs(vx)*dt, 1 + abs(vy)*dt, 1 + abs(w)*dt]
u = [0, 0, 0]
self.kf.predict(A, u)
elif False and self.cf1_vel:
A = np.eye(4)
A[0][2] = 1.0/self.update_freq
A[1][3] = 1.0/self.update_freq
self.kf.predict(A, u)
else:
A = [1, 1, 1]
u = [0, 0, 0]
self.kf.predict(A, u)
"""
if self.cf1_pose:
p = PoseWithCovarianceStamped()
p.header = self.cf1_pose.header # correct to use cf1/odom as frame_id???
p.pose.pose = self.cf1_pose.pose
p.pose.covariance[0] = self.kf.cov[0][0]
#p.pose.covariance[1] = self.kf.cov[0][0]*self.kf.cov[1][1]
#p.pose.covariance[6] = self.kf.cov[0][0]*self.kf.cov[1][1]
p.pose.covariance[7] = self.kf.cov[1][1]
p.pose.covariance[-1] = self.kf.cov[2][2]
self.cf1_pose_cov_pub.publish(p)
rate.sleep()
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 f(msg):
if _type == NavSatFix and msg.status.status < 0:
# No fix
return
self.last_msg[index] = rospy.Time.now()
if self.active:
if index <= self.active_index:
self.active_index = index
else:
dt = (rospy.Time.now() -
self.last_msg[self.active_index]).to_sec()
if dt > self.timeout:
rospy.logwarn(
'Switch to pose source {0} from {1}'.format(
self.topics[index],
self.topics[self.active_index]))
self.active_index = index
if index == self.active_index:
if _type == PoseWithCovarianceStamped:
pose_c = msg
elif _type == PoseStamped:
pose_c = PoseWithCovarianceStamped()
pose_c.header = msg.header
pose_c.pose.pose = msg.pose
pose_c.pose.covariance = cov
elif _type == NavSatFix:
pose_c = PoseWithCovarianceStamped()
pose_c.header.frame_id = 'utm'
pose_c.pose.pose.orientation.w = 1.0
x, y, _, _ = utm.from_latlon(msg.latitude, msg.longitude)
z = msg.altitude
position = pose_c.pose.pose.position
position.x = x
position.y = y
position.z = z
if cov is None:
gps_cov = msg.position_covariance
else:
gps_cov = cov
for i in range(3):
for j in range(3):
pose_c.pose.covariance[6 * i + j] = gps_cov[3 * i +
j]
else:
return
# Solve issue with not synchronized publishers
pose_c.header.stamp = rospy.Time.now()
self.pub.publish(pose_c)
def state2pose(self, state, cov):
output = PoseWithCovarianceStamped()
output.pose.pose.position.x = state[0]
output.pose.pose.position.y = state[1]
cov_zeros = np.zeros((36,))
cov_zeros[0] = cov[0,0]
cov_zeros[1] = cov[0,1]
cov_zeros[6] = cov[1,0]
cov_zeros[7] = cov[1,1]
output.pose.covariance = cov_zeros.flatten()
h = std_msgs.msg.Header()
h.stamp = rospy.Time.now()
h.frame_id = 'odom'
output.header = h
return output
def fix(self):
#self.threadC.acquire()
tmp1 = PoseWithCovarianceStamped()
tmp1.header = self.odomFilter.header
tmp1.pose.pose.position.x = self.world.pose.pose.position.x
tmp1.pose.pose.position.y = self.odomFilter.pose.pose.position.y
tmp1.pose.pose.position.z = self.odomFilter.pose.pose.position.z
tmp1.pose.pose.orientation.x = self.odomFilter.pose.pose.orientation.x
tmp1.pose.pose.orientation.y = self.odomFilter.pose.pose.orientation.y
tmp1.pose.pose.orientation.z = self.odomFilter.pose.pose.orientation.z
tmp1.pose.pose.orientation.w = self.odomFilter.pose.pose.orientation.w
tmp1.pose.covariance = self.odomFilter.pose.covariance
#tmp1 = msg
#tmp1.header.frame_id = "odom"
#self.br.sendTransform((0.0,0.0,0.0),(0.0,0.0,0.0,1.0),rospy.Time.now(),"base_link","odom")
self.pub.publish(tmp1)
def state2pose(self, state, cov):
output = PoseWithCovarianceStamped()
output.pose.pose.position.x = state[0]
output.pose.pose.position.y = state[1]
cov_zeros = np.zeros((36, ))
cov_zeros[0] = cov[0, 0]
cov_zeros[1] = cov[0, 1]
cov_zeros[6] = cov[1, 0]
cov_zeros[7] = cov[1, 1]
output.pose.covariance = cov_zeros.flatten()
h = std_msgs.msg.Header()
h.stamp = rospy.Time.now()
h.frame_id = 'odom'
output.header = h
return output
def correct_strapdown(self, header, x_nav, P_nav, orientation,
orientation_cov):
msg = PoseWithCovarianceStamped()
msg.header = header
msg.header.frame_id = "odom"
# Transform
msg.pose.pose.position.x = x_nav[0, 0]
msg.pose.pose.position.y = x_nav[1, 0]
msg.pose.pose.position.z = x_nav[2, 0]
msg.pose.pose.orientation = orientation
new_cov = np.zeros((6, 6))
new_cov[:3, :3] = P_nav[:3, :3] # TODO add full cross correlations
new_cov[3:, 3:] = orientation_cov[3:, 3:]
msg.pose.covariance = list(new_cov.flatten())
self.intersection_pub.publish(msg)
def path_plan_cb(self, data):
# Gets path plan from path_planner node
if self.type_ == "Follower":
self.start_pos = data.follower.initial
self.end_pos = data.follower.goal
self.transfer_start_pos = data.follower.line_start
self.transfer_end_pos = data.follower.line_end
elif self.type_ == "Leader":
self.start_pos = data.leader.initial
self.end_pos = data.leader.goal
self.transfer_start_pos = data.leader.line_start
self.transfer_end_pos = data.leader.line_end
print("Path plan received")
pose_stamped = xytheta_to_pose(*self.start_pos)
msg = PoseWithCovarianceStamped()
msg.header = pose_stamped.header
msg.pose.pose = pose_stamped.pose
def do_transform_pose_with_covariance_stamped(pose, transform):
f = transform_to_kdl(transform) * PyKDL.Frame(
PyKDL.Rotation.Quaternion(
pose.pose.pose.orientation.x, pose.pose.pose.orientation.y,
pose.pose.pose.orientation.z, pose.pose.pose.orientation.w),
PyKDL.Vector(pose.pose.pose.position.x, pose.pose.pose.position.y,
pose.pose.pose.position.z))
res = PoseWithCovarianceStamped()
res.pose.pose.position.x = f.p[0]
res.pose.pose.position.y = f.p[1]
res.pose.pose.position.z = f.p[2]
(res.pose.pose.orientation.x, res.pose.pose.orientation.y,
res.pose.pose.orientation.z,
res.pose.pose.orientation.w) = f.M.GetQuaternion()
res.pose.covariance = pose.pose.covariance
res.header = transform.header
return res
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 plan_base(self, goal):
#find the current base pose from the planning scene
robot_state = self.psi.get_robot_state()
starting_state = PoseStamped()
starting_state.header.frame_id = robot_state.multi_dof_joint_state.frame_ids[
0]
starting_state.pose = robot_state.multi_dof_joint_state.poses[0]
goal_robot_state = copy.deepcopy(robot_state)
goal_robot_state.multi_dof_joint_state.poses[0] = goal.pose
start_markers = vt.robot_marker(robot_state, ns='base_starting_state')
goal_markers = vt.robot_marker(goal_robot_state,
ns='base_goal_state',
g=1,
b=0)
self.publish(start_markers)
self.publish(goal_markers)
#plan
good_plan = False
while not good_plan:
try:
start_time = time.time()
self.base_plan_service(start=starting_state, goal=goal)
end_time = time.time()
good_plan = True
except:
rospy.loginfo(
'Having trouble probably with starting state. Sleeping and trying again'
)
rospy.sleep(0.2)
#it's apparently better just to do the trajectory
#than fool with the planning scene
goal_cov = PoseWithCovarianceStamped()
goal_cov.header = goal.header
goal_cov.pose.pose = goal.pose
self.pose_pub.publish(goal_cov)
self.psi.reset()
# self.base.move_to(goal.pose.position.x, goal.pose.position.y,
# 2.0*np.arcsin(goal.pose.orientation.z))
return (end_time - start_time)
def publish_pose(self, position=[0, 0, 0], orientation=[0, 0, 0, 1], error=None):
msg = PoseStamped()
msg.header.frame_id = self.frame_id
msg.header.stamp = rospy.Time.now()
msg.pose.position = Point(*position)
msg.pose.orientation = Quaternion(*orientation)
self.pose_pub.publish(msg)
self.pose_pub_stat.tick(msg.header.stamp)
pmsg = PoseWithCovarianceStamped()
pmsg.header = msg.header
pmsg.pose.pose = msg.pose
pmsg.pose.covariance = self.pose_cov
self.pose_cov_pub.publish(pmsg)
if error is not None:
e = h_error(error)
self.error_pub.publish(e)
def publish_position(self, pos, ps_pub, ps_cov_pub, cov):
x, y = pos[0], pos[1]
if len(pos) > 2:
z = pos[2]
else:
z = 0
ps = PoseStamped()
ps_cov = PoseWithCovarianceStamped()
ps.pose.position.x = x
ps.pose.position.y = y
ps.pose.position.z = z
ps.header.frame_id = self.frame_id
ps.header.stamp = rospy.get_rostime()
ps_cov.header = ps.header
ps_cov.pose.pose = ps.pose
ps_cov.pose.covariance = cov
ps_pub.publish(ps)
ps_cov_pub.publish(ps_cov)
def publish_position(self, pos, ps_pub, ps_cov_pub, cov):
x, y = pos[0], pos[1]
if len(pos) > 2:
z = pos[2]
else:
z = 0
ps = PoseStamped()
ps_cov = PoseWithCovarianceStamped()
ps.pose.position.x = x
ps.pose.position.y = y
ps.pose.position.z = z
ps.header.frame_id = self.frame_id
ps.header.stamp = rospy.get_rostime()
ps_cov.header = ps.header
ps_cov.pose.pose = ps.pose
ps_cov.pose.covariance = cov
ps_pub.publish(ps)
ps_cov_pub.publish(ps_cov)
def spin(self):
rate = rospy.Rate(self.update_freq)
self.has_transformed = False # to check if initial transform with kalman gain 1 result in good transform
while not rospy.is_shutdown():
self.broadcaster.sendTransform(self.last_transform)
self.last_transform.header.stamp = rospy.Time.now()
A = np.diag([1, 1, 1, 1, 1, 1])
if False and self.cf1_vel:
v = np.array([
self.cf1_vel.twist.linear.x, self.cf1_vel.twist.linear.y,
self.cf1_vel.twist.linear.z, self.cf1_vel.twist.angular.x,
self.cf1_vel.twist.angular.y, self.cf1_vel.twist.angular.z
])
self.kf.predict(A, v)
else:
self.kf.predict(A)
if self.measurement_msg:
if not self.has_transformed:
map_to_odom = self.update(self.measurement_msg)
if map_to_odom:
self.last_transform = map_to_odom
#self.has_transformed = True
#norm = np.linalg.norm(self.kf.cov)
#print("Cov norm: {}".format(norm))
self.converged_pub.publish(Bool(self.kf.has_converged()))
if self.cf1_pose:
p = PoseWithCovarianceStamped()
p.header = self.cf1_pose.header # correct to use cf1/odom as frame_id???
p.pose.pose = self.cf1_pose.pose
p.pose.covariance[0] = self.kf.cov[0][0]
#p.pose.covariance[1] = self.kf.cov[0][1]
#p.pose.covariance[6] = self.kf.cov[1][0]
p.pose.covariance[7] = self.kf.cov[1][1]
p.pose.covariance[-1] = self.kf.cov[5][5]
self.cf1_pose_cov_pub.publish(p)
rate.sleep()
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 control_gps(self, data_gps):
if not self.gps_init:
rospy.loginfo('GPS is ON')
self.gps_init = True
llh = [data_gps.latitude, data_gps.longitude, data_gps.altitude]
ned = self.ned.geodetic2ned(llh)
msg = PoseWithCovarianceStamped()
msg.header = data_gps.header
msg.header.frame_id = 'map'
msg.pose.pose.position.x = ned[1]
msg.pose.pose.position.y = ned[0]
msg.pose.pose.position.z = ned[2]
msg.pose.pose.orientation.w = 1.0
msg.pose.covariance[0] = data_gps.position_covariance[0]
msg.pose.covariance[7] = data_gps.position_covariance[4]
msg.pose.covariance[14] = data_gps.position_covariance[8]
self.gps_pose_pub.publish(msg)
def getDistToHuman(self, human_pose_in):
"""Returns distance between human track and robot."""
human_pose_base = None
dist = np.inf
hpose_out = None
hpose = PoseStamped()
hpose.header = human_pose_in.header
hpose.pose = human_pose_in.pose.pose
human_pose_base = self.transformPoseSt(hpose, self.base_frame_id)
if (human_pose_base.header.frame_id == self.base_frame_id):
dist = np.sqrt((human_pose_base.pose.position.x**2) +
(human_pose_base.pose.position.y**2))
hpose_out = PoseWithCovarianceStamped()
hpose_out.header = human_pose_base.header
hpose_out.pose.pose = human_pose_base.pose
hpose_out.pose.covariance = human_pose_in.pose.covariance
return (dist, hpose_out)
def spin_publisher(self):
r = rospy.Rate(10)
while not rospy.is_shutdown():
h = Header(frame_id='map', stamp=rospy.Time.now())
p = PoseWithCovarianceStamped()
p.header = h
p.pose.pose.position.x = self.x
p.pose.pose.position.y = self.y
p.pose.pose.orientation = self.q
self.pose_pub.publish(p)
t = TransformStamped()
t.header = h
t.child_frame_id = 'base_footprint'
t.transform.translation.x = self.x
t.transform.translation.y = self.y
t.transform.translation.z = 0.0
t.transform.rotation = self.q
self.tf_pub.sendTransform(t)
r.sleep()
def publish_pose(trfm_msg, map_pub):
pub_msg = PoseWithCovarianceStamped()
pub_msg.header = trfm_msg.header
[
pub_msg.pose.pose.position.x, pub_msg.pose.pose.position.y,
pub_msg.pose.pose.position.z
] = [
trfm_msg.transform.translation.x, trfm_msg.transform.translation.y,
trfm_msg.transform.translation.z
]
[
pub_msg.pose.pose.orientation.x, pub_msg.pose.pose.orientation.y,
pub_msg.pose.pose.orientation.z, pub_msg.pose.pose.orientation.w
] = [
trfm_msg.transform.rotation.x, trfm_msg.transform.rotation.y,
trfm_msg.transform.rotation.z, trfm_msg.transform.rotation.w
]
map_pub.publish(pub_msg)
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 spin(self):
rate = rospy.Rate(self.update_freq)
self.has_transformed = False # to check if initial transform with kalman gain 1 result in good transform
while not rospy.is_shutdown():
self.broadcaster.sendTransform(self.last_transform)
self.last_transform.header.stamp = rospy.Time.now()
A = np.diag([1, 1, 1, 1, 1, 1])
if False: #self.cf1_vel:
v = np.array([
self.cf1_vel.twist.linear.x, self.cf1_vel.twist.linear.y,
self.cf1_vel.twist.linear.z, self.cf1_vel.twist.angular.x,
self.cf1_vel.twist.angular.y, self.cf1_vel.twist.angular.z
])
self.kf.predict(A, v)
else:
self.kf.predict(A)
if self.measurement_msg:
map_to_odom = self.update(self.measurement_msg)
if map_to_odom:
self.last_transform = map_to_odom
self.converged_pub.publish(Bool(self.kf.has_converged()))
if self.cf1_pose:
p = PoseWithCovarianceStamped()
p.header = self.cf1_pose.header
p.pose.pose = self.cf1_pose.pose
#self.tf_buf.transform(p, "map")
p.pose.covariance[0] = self.kf.cov[0][0] # x
p.pose.covariance[1] = self.kf.cov[0][1] # xy
p.pose.covariance[6] = self.kf.cov[1][0] # yx
p.pose.covariance[7] = self.kf.cov[1][1] # y
p.pose.covariance[-1] = self.kf.cov[5][5] # z angle
self.cf1_pose_cov_pub.publish(p)
rate.sleep()
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 state2pose_seq(self, state, cov):
robot_output = PoseWithCovarianceStamped()
robot_output.pose.pose.position.x = state[0]
robot_output.pose.pose.position.y = state[1]
robot_cov = np.zeros((36,))
robot_cov[0] = cov[0,0]
robot_cov[1] = cov[0,1]
robot_cov[6] = cov[1,0]
robot_cov[7] = cov[1,1]
robot_output.pose.covariance = robot_cov.flatten()
oppo = tf.transformations.quaternion_from_euler(0,0,self.yaw)
robot_output.pose.pose.orientation.x = oppo[0]
robot_output.pose.pose.orientation.y = oppo[1]
robot_output.pose.pose.orientation.z = oppo[2]
robot_output.pose.pose.orientation.w = oppo[3]
h = std_msgs.msg.Header()
h.stamp = self.t #rospy.Time.now()
h.frame_id = 'odom'
robot_output.header = h
return robot_output
def getDistToHuman(self, human_pose_in):
"""Returns distance between human track and robot."""
dist = np.inf
hpose_out = None
if (self.robotPoseSt):
hpose = PoseStamped()
hpose.header = human_pose_in.header
hpose.pose = human_pose_in.pose.pose
human_pose_rob = self.transformPoseSt(hpose, self.robotPoseSt.header.frame_id)
human_pose_glob = self.transformPoseSt(hpose, self.global_frame_id)
if (human_pose_rob) and (human_pose_glob):
dist = np.sqrt((human_pose_rob.pose.position.x ** 2) +
(human_pose_rob.pose.position.y ** 2))
hpose_out = PoseWithCovarianceStamped()
hpose_out.header = human_pose_glob.header
hpose_out.pose.pose = human_pose_glob.pose
hpose_out.pose.covariance = human_pose_in.pose.covariance
return (dist, hpose_out)
def publish_pose(map_frame_id, base_frame_id):
global buf
while True:
try:
transform = buf.lookup_transform(map_frame_id, base_frame_id,
rospy.Time(0), rospy.Duration(3))
break
except tf2_ros.TransformException as e:
rospy.logerr(e)
continue
pose = PoseWithCovarianceStamped()
pose.header = transform.header
p = pose.pose.pose
p.position.x = transform.transform.translation.x
p.position.y = transform.transform.translation.y
p.position.z = transform.transform.translation.z
p.orientation.x = transform.transform.rotation.x
p.orientation.y = transform.transform.rotation.y
p.orientation.z = transform.transform.rotation.z
p.orientation.w = transform.transform.rotation.w
initialpose_pub.publish(pose)
def publish_pose(self, particle_avg):
# Update pose and TF
self.pose_est = np.array(particle_avg)
#Generate pose
q = tf.transformations.quaternion_from_euler(0, 0, self.pose_est[2])
header = Header(stamp=rospy.Time.now(), frame_id="map")
pose = Pose(position=Point(x=self.pose_est[0],
y=self.pose_est[1],
z=.127),
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([
.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, .075
])**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 poseMessage(self):
newframe = "map"
posemsg = PoseWithCovarianceStamped()
poseStamped = PoseStamped()
poseStamped.header = self.marker_pose.header
poseStamped.pose = self.marker_pose.pose.pose
try:
poseStamped = self.transformer.transformPose(newframe, poseStamped)
except tf.ConnectivityException as ex:
rospy.logerr(ex)
raise Exception("Transform failed")
except tf.LookupException as ex:
rospy.logerr(ex)
raise Exception("Transform failed")
except tf.ExtrapolationException as ex:
rospy.logerr(ex)
raise Exception("Transform failed")
posemsg.header = poseStamped.header
posemsg.pose.pose = poseStamped.pose # Pose
posemsg.pose.covariance = self.marker_pose.pose.covariance
return posemsg
def publish_uav_target_pose():
if (gun_pose is None):
rospy.logwarn_throttle(
10.0,
rospy.get_name() + " : Gun pose not yet received")
else:
target_pose = gun_pose
targetOdom = Odometry()
# Populating Odometry msg
global sequence_number
targetOdom.header.frame_id = "odom"
targetOdom.header.stamp = rospy.get_rostime()
targetOdom.header.seq = sequence_number
sequence_number += 1
targetOdom.pose.pose = target_pose
uav_target_odom_pub.publish(targetOdom)
targetPose = PoseWithCovarianceStamped()
targetPose.header = targetOdom.header
targetPose.pose.pose = target_pose
uav_target_pose_pub.publish(targetPose)
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 poseMessage(self):
newframe = "map"
posemsg = PoseWithCovarianceStamped()
poseStamped = PoseStamped()
poseStamped.header = self.marker_pose.header
poseStamped.pose = self.marker_pose.pose.pose
try:
poseStamped = self.transformer.transformPose(newframe,poseStamped)
except tf.ConnectivityException as ex:
rospy.logerr(ex)
raise Exception("Transform failed")
except tf.LookupException as ex:
rospy.logerr(ex)
raise Exception("Transform failed")
except tf.ExtrapolationException as ex:
rospy.logerr(ex)
raise Exception("Transform failed")
posemsg.header = poseStamped.header
posemsg.pose.pose = poseStamped.pose # Pose
posemsg.pose.covariance = self.marker_pose.pose.covariance
return posemsg
def measSensorLoopTimerCallback(self, timer_msg):
# Get time
time_stamp_current = rospy.Time.now()
#
if (self.flag_robot_pose_set == False):
return
# Computing the measurement
#
meas_posi = np.zeros((3, ), dtype=float)
meas_atti_quat = ars_lib_helpers.Quaternion.zerosQuat()
# Position
meas_posi[0] = self.robot_posi[0] + np.random.normal(
loc=0.0, scale=math.sqrt(self.cov_meas_pos['x']))
meas_posi[1] = self.robot_posi[1] + np.random.normal(
loc=0.0, scale=math.sqrt(self.cov_meas_pos['y']))
meas_posi[2] = self.robot_posi[2] + np.random.normal(
loc=0.0, scale=math.sqrt(self.cov_meas_pos['z']))
# Attitude
noise_atti_ang = np.zeros((3, ), dtype=float)
noise_atti_ang[0] = np.random.normal(loc=0.0,
scale=math.sqrt(
self.cov_meas_att['x']))
noise_atti_ang[1] = np.random.normal(loc=0.0,
scale=math.sqrt(
self.cov_meas_att['y']))
noise_atti_ang[2] = np.random.normal(loc=0.0,
scale=math.sqrt(
self.cov_meas_att['z']))
noise_atti_quat_tf = tf.transformations.quaternion_from_euler(
noise_atti_ang[0],
noise_atti_ang[1],
noise_atti_ang[2],
axes='sxyz')
noise_atti_quat = np.roll(noise_atti_quat_tf, 1)
meas_atti_quat = ars_lib_helpers.Quaternion.quatProd(
self.robot_atti_quat, noise_atti_quat)
# Covariance
meas_cov_posi = np.diag([
self.cov_meas_pos['x'], self.cov_meas_pos['y'],
self.cov_meas_pos['z'], self.cov_meas_att['x'],
self.cov_meas_att['y'], self.cov_meas_att['z']
])
# Filling the message
#
meas_header_msg = Header()
meas_robot_pose_msg = Pose()
meas_robot_pose_stamp_msg = PoseStamped()
meas_robot_pose_cov_msg = PoseWithCovarianceStamped()
#
meas_header_msg.frame_id = self.robot_frame_id
meas_header_msg.stamp = self.robot_pose_timestamp
# Position
meas_robot_pose_msg.position.x = meas_posi[0]
meas_robot_pose_msg.position.y = meas_posi[1]
meas_robot_pose_msg.position.z = meas_posi[2]
# Attitude
meas_robot_pose_msg.orientation.w = meas_atti_quat[0]
meas_robot_pose_msg.orientation.x = meas_atti_quat[1]
meas_robot_pose_msg.orientation.y = meas_atti_quat[2]
meas_robot_pose_msg.orientation.z = meas_atti_quat[3]
#
meas_robot_pose_stamp_msg.header = meas_header_msg
meas_robot_pose_stamp_msg.pose = meas_robot_pose_msg
#
meas_robot_pose_cov_msg.header = meas_header_msg
meas_robot_pose_cov_msg.pose.covariance = meas_cov_posi.reshape(
(36, 1))
meas_robot_pose_cov_msg.pose.pose = meas_robot_pose_msg
#
self.meas_robot_pose_pub.publish(meas_robot_pose_stamp_msg)
self.meas_robot_pose_cov_pub.publish(meas_robot_pose_cov_msg)
#
return
def point_tracker(self, pointcloud):
# Assume there there is, at most, 1 object (Bonus: get code to work for multiple objects! Hint: use a new kalman filter for each object)
if len(pointcloud.points) > 0:
point = pointcloud.points[0]
measurement = np.matrix([point.x, point.y]).T # note: the shape is critical; this is a matrix, not an array (so that matrix mult. works)
else:
measurement = None
nmeasurements = 2
if measurement is None: # propagate a blank measurement
m = np.matrix([np.nan for i in range(2) ]).T
xhat, P, K = self.kalman_filter.update( None )
else: # propagate the measurement
xhat, P, K = self.kalman_filter.update( measurement ) # run kalman filter
### Publish the results as a simple array
float_time = float(pointcloud.header.stamp.secs) + float(pointcloud.header.stamp.nsecs)*1e-9
x = xhat.item(0) # xhat is a matrix, .item() gives you the actual value
xdot = xhat.item(1)
y = xhat.item(2)
ydot = xhat.item(3)
p_vector = P.reshape(16).tolist()[0]
data = [float_time, x, xdot, y, ydot]
data.extend(p_vector)
float64msg = Float64MultiArray()
float64msg.data = data
now = rospy.get_time()
self.time_start = now
self.pubTrackedObjects_Float64MultiArray.publish( float64msg )
###
### Publish the results as a ROS type pose (positional information)
# see: http://docs.ros.org/jade/api/geometry_msgs/html/msg/PoseWithCovarianceStamped.html
pose = PoseWithCovarianceStamped()
pose.header = pointcloud.header
pose.pose.pose.position.x = x
pose.pose.pose.position.y = y
pose.pose.pose.position.z = 0
pose.pose.pose.orientation.x = 0
pose.pose.pose.orientation.y = 0
pose.pose.pose.orientation.z = 0
pose.pose.pose.orientation.w = 0
x_x = P[0,0]
x_y = P[0,2]
y_y = P[2,2]
position_covariance = [x_x, x_y, 0, 0, 0, 0, x_y, y_y, 0, 0, 0, 0]
position_covariance.extend([0]*24)
# position_covariance is the row-major representation of a 6x6 covariance matrix
pose.pose.covariance = position_covariance
self.pubTrackedObjects_Pose.publish( pose )
###
### Publish the results as a ROS type twist (velocity information)
# see: http://docs.ros.org/jade/api/geometry_msgs/html/msg/PoseWithCovarianceStamped.html
twist = TwistWithCovarianceStamped()
twist.header = pointcloud.header
twist.twist.twist.linear.x = xdot
twist.twist.twist.linear.y = ydot
twist.twist.twist.linear.z = 0
twist.twist.twist.angular.x = 0
twist.twist.twist.angular.y = 0
twist.twist.twist.angular.z = 0
dx_dx = P[1,1]
dx_dy = P[1,3]
dy_dy = P[3,3]
velocity_covariance = [x_x, x_y, 0, 0, 0, 0, x_y, y_y, 0, 0, 0, 0]
velocity_covariance.extend([0]*24)
# position_covariance is the row-major representation of a 6x6 covariance matrix
twist.twist.covariance = velocity_covariance
self.pubTrackedObjects_Twist.publish( twist )
#!/usr/bin/env python
import rospy
from ms5837.msg import ms5837_data
from std_msgs.msg import Header
from geometry_msgs.msg import PoseWithCovarianceStamped
pub = rospy.Publisher('ms5837_pose', PoseWithCovarianceStamped, queue_size=3)
global zero_value
zero_value = 0
ros_msg = PoseWithCovarianceStamped()
header = Header()
header.frame_id = 'ms5837_pose_data'
header.stamp = rospy.Time.now()
ros_msg.header = header
ros_msg.pose.pose.position.z = 0
ros_msg.pose.covariance[8] = 0.0001349092722
def callback(msg):
header.frame_id = 'ms5837_pose_data'
header.stamp = rospy.Time.now()
ros_msg.header = header
ros_msg.pose.pose.position.z = msg.depth - zero_value
pub.publish(ros_msg)
def zeroing():
# zeroing the message
zero_value += ros_msg.pose.pose.position.z
def fusion(self):
print("Here",self.lidar_LOC,self.camc_LOC,self.camg_LOC)
if self.lidar_LOC > 0 or self.camc_LOC > 0 or self.camg_LOC > 0:
fused_msg = PoseWithCovarianceStamped()
COV=fused_msg.pose.covariance
h = std_msgs.msg.Header()
h.stamp = rospy.Time.now()
h.frame_id = 'base_link'
fused_msg.header = h
if self.lidar_LOC > 0 and self.camc_LOC > 0 and self.camg_LOC > 0:
print("Fuse all sensors.")
fused_msg.pose.pose.position.x = (self.lidar_x+self.camc_x+self.camg_x)/3
fused_msg.pose.pose.position.y = (self.lidar_y+self.camc_y+self.camg_y)/3
fused_msg.pose.pose.position.z = self.lidar_LOC+self.camc_LOC+self.camg_LOC
COV[0]=self.lidar_cov[0]*self.camc_cov[0]*self.camg_cov[0]
COV[1]=self.lidar_cov[1]*self.camc_cov[1]*self.camg_cov[1]
COV[6]=self.lidar_cov[2]*self.camc_cov[2]*self.camg_cov[2]
COV[7]=self.lidar_cov[3]*self.camc_cov[3]*self.camg_cov[3]
fused_msg.pose.covariance=COV
elif self.lidar_LOC > 0 and self.camc_LOC > 0:
print("Fuse Lidar and color.")
fused_msg.pose.pose.position.x = (self.lidar_x+self.camc_x)/2
fused_msg.pose.pose.position.y = (self.lidar_y+self.camc_y)/2
fused_msg.pose.pose.position.z = self.lidar_LOC+self.camc_LOC
COV[0]=self.lidar_cov[0]*self.camc_cov[0]
COV[1]=self.lidar_cov[1]*self.camc_cov[1]
COV[6]=self.lidar_cov[2]*self.camc_cov[2]
COV[7]=self.lidar_cov[3]*self.camc_cov[3]
fused_msg.pose.covariance=COV
elif self.lidar_LOC > 0 and self.camg_LOC > 0:
print("Fuse Lidar and geometry.")
fused_msg.pose.pose.position.x = (self.lidar_x+self.camg_x)/2
fused_msg.pose.pose.position.y = (self.lidar_y+self.camg_y)/2
fused_msg.pose.pose.position.z = self.lidar_LOC+self.camg_LOC
COV[0]=self.lidar_cov[0]*self.camg_cov[0]
COV[1]=self.lidar_cov[1]*self.camg_cov[1]
COV[6]=self.lidar_cov[2]*self.camg_cov[2]
COV[7]=self.lidar_cov[3]*self.camg_cov[3]
fused_msg.pose.covariance=COV
elif self.camc_LOC > 0 and self.camg_LOC > 0:
print("Fuse Color and Geometry.")
fused_msg.pose.pose.position.x = (self.camc_x+self.camg_x)/2
fused_msg.pose.pose.position.y = (self.camc_y+self.camg_y)/2
fused_msg.pose.pose.position.z = self.camc_LOC+self.camg_LOC
COV[0]=self.camc_cov[0]*self.camg_cov[0]
COV[1]=self.camc_cov[1]*self.camg_cov[1]
COV[6]=self.camc_cov[2]*self.camg_cov[2]
COV[7]=self.camc_cov[3]*self.camg_cov[3]
fused_msg.pose.covariance=COV
elif self.lidar_LOC > 0:
print("Only Lidar.")
fused_msg.pose.pose.position.x = self.lidar_x
fused_msg.pose.pose.position.y = self.lidar_y
fused_msg.pose.pose.position.z = self.lidar_LOC
COV[0]=self.lidar_cov[0]
COV[1]=self.lidar_cov[1]
COV[6]=self.lidar_cov[2]
COV[7]=self.lidar_cov[3]
fused_msg.pose.covariance=COV
elif self.camc_LOC > 0:
print("Only Color.")
fused_msg.pose.pose.position.x = self.camc_x
fused_msg.pose.pose.position.y = self.camc_y
fused_msg.pose.pose.position.z = self.camc_LOC
COV[0]=self.camc_cov[0]
COV[1]=self.camc_cov[1]
COV[6]=self.camc_cov[2]
COV[7]=self.camc_cov[3]
fused_msg.pose.covariance=COV
elif self.camg_LOC > 0:
print("Only Geometry.")
fused_msg.pose.pose.position.x = self.camg_x
fused_msg.pose.pose.position.y = self.camg_y
fused_msg.pose.pose.position.z = self.camg_LOC
COV[0]=self.camg_cov[0]
COV[1]=self.camg_cov[1]
COV[6]=self.camg_cov[2]
COV[7]=self.camg_cov[3]
fused_msg.pose.covariance=COV
print(fused_msg)
self.target_pub.publish(fused_msg)
def publish_pose(self, last_ekf_state, pre_update_ekf_state,
pre_update_ekf_cov, last_true_position, last_true_theta,
current_true_odom):
print("Publishing EKF pose! Time: {0}".format(
rospy.get_rostime().to_sec()))
odom = Odometry()
header = odom.header
header.seq = self.seq_num
self.seq_num += 1
header.stamp = rospy.get_rostime()
header.frame_id = 'map'
position = odom.pose.pose.position
position.x, position.y = self.state[0], self.state[1]
orientation = odom.pose.pose.orientation
quat = quat_from_rpy_rad(0.0, 0.0, self.state[2])
orientation.x, orientation.y, orientation.z, orientation.w = quat
# how the hell do I convert covariance between heading and other values
# to covariance between quaternion and other values??
# => don't actually care about covariance between anything other than x, y in my applcation
# so just set covariances for quaternion to 0?
cov = np.zeros((6, 6))
cov[:3, :3] = self.cov[:3, :3]
odom.pose.covariance = list(cov.ravel())
# insert velocity information
linear_vel = odom.twist.twist.linear
linear_vel.x, linear_vel.y, linear_vel.z = self.state[3], self.state[
4], 0.0
angular_vel = odom.twist.twist.angular
angular_vel.z = self.state[5] # TODO check this is right!
# don't bother setting covariance since I don't understand it for angular vel's and I don't actually use it
self.odom_publisher.publish(odom)
if self.publish_rviz_pose:
pose = PoseWithCovarianceStamped()
pose.header = header
pose.pose = odom.pose
self.pose_publisher.publish(pose)
# ----publish a SimDataMessage for the sim data collector to deal with---
if last_true_position is None:
return
msg = SimulationDataMsg()
msg.true_odom = current_true_odom
p = msg.last_true_pos
p.x, p.y, p.z = last_true_position
msg.last_true_heading = last_true_theta
msg.last_ekf_state = last_ekf_state.tolist()
# NOTE: we may have had a camera update here
# so we want to write the position before jumping due to camera update!
position = msg.ekf_odom.pose.pose.position
position.x, position.y = pre_update_ekf_state[0], pre_update_ekf_state[
1]
orientation = msg.ekf_odom.pose.pose.orientation
quat = quat_from_rpy_rad(0.0, 0.0, pre_update_ekf_state[2])
orientation.x, orientation.y, orientation.z, orientation.w = quat
cov = np.zeros((6, 6))
cov[:3, :3] = pre_update_ekf_cov[:3, :3]
msg.ekf_odom.pose.covariance = list(pre_update_ekf_cov.ravel())
msg.ekf_odom.header.stamp = rospy.get_rostime()
# insert velocity information
linear_vel = msg.ekf_odom.twist.twist.linear
linear_vel.x, linear_vel.y, linear_vel.z = pre_update_ekf_state[
3], pre_update_ekf_state[4], 0.0
angular_vel = msg.ekf_odom.twist.twist.angular
angular_vel.z = pre_update_ekf_state[5]
# insert camera update if had any
if self.last_sensor_position is None:
msg.has_camera_update = False
else:
msg.has_camera_update = True
# save the saved values into the message
# have to do it this way because
# ROS may interrupt/receive new data
# at any time it seems...
msg_pos = msg.camera_update.position
msg_pos.x, msg_pos.y = self.last_sensor_position
msg.camera_update.covariance = self.last_sensor_cov.ravel().tolist(
)
self.to_sim_data_collector.publish(msg)
def encoder_interface():
test_pub = rospy.Publisher('test_chatter', String, queue_size=10)
pose_pub = rospy.Publisher('test_pose', PoseWithCovarianceStamped, queue_size=10)
rospy.init_node('encoder_interface', anonymous=True)
rate = rospy.Rate(10) # 10hz
ser = serial.Serial(
port='/dev/ttyUSB3',
baudrate=115200,
bytesize=8,
parity=serial.PARITY_NONE,
stopbits=1,
#timeout=2,
xonxoff=False,
rtscts=False,
#writetimeout=2,
dsrdtr=False,
#interchartimeout=None
)
#9600,8,serial.PARITY_NONE,1,2,False,True,2,False,None
#ser = serial.Serial(
# port='/dev/ttyUSB1',
# baudrate=9600,
# parity='N', #serial.PARITY_ODD #EVEN
# stopbits=1, #serial.STOPBITS_ONE #TWO
# bytesize=8,
# xonxoff=1,
# rtscts=0
#)
while not rospy.is_shutdown():
#test_chatter
hello_str = "hello world %s" % rospy.get_time()
#rospy.loginfo(hello_str)
test_pub.publish(hello_str)
#test_pose
pose = PoseWithCovarianceStamped()
pose.header = std_msgs.msg.Header()
pose.header.frame_id = "odom"
pose.header.stamp = rospy.Time.now()
pose.pose.pose.orientation.w = 1
pose.pose.pose.orientation.x = 1
pose.pose.pose.orientation.y = 1
pose.pose.pose.orientation.z = 1
pose.pose.pose.position.x = 1 + random.random()
pose.pose.pose.position.y = random.random()
pose.pose.pose.position.z = random.random()
try:
byte = ser.read()
#print chr(int(byte))
print repr(byte)
except Exception, e:
#print "Failed to read: ", e
continue
pose_pub.publish(pose)
rate.sleep()
def poseCallback(mag):
pose = PoseWithCovarianceStamped()
pose.header = mag.header
pose.pose.orientation = Quaternion(mag.x, mag.y, mag.z)
# TODO: https://cdn-shop.adafruit.com/datasheets/AN203_Compass_Heading_Using_Magnetometers.pdf
pub.publish(pose)
def publish_tf(self, pose, stamp=None):
""" Publish a tf for the car. This tells ROS where the car is with respect to the map. """
if stamp == None:
stamp = rospy.Time.now()
# also publish odometry to facilitate getting the localization pose
if self.PUBLISH_ODOM:
odom = Odometry()
odom.header = Utils.make_header("map", stamp)
odom.child_frame_id = "base_link"
odom.pose.pose.position.x = pose[0]
odom.pose.pose.position.y = pose[1]
odom.pose.pose.orientation = Utils.angle_to_quaternion(pose[2])
self.odom_pub.publish(odom)
#-------------------------- SAMUEL - added - Publish PoseWithCovariance Msg ---------------------------------
if self.PUBLISH_POSE_COVARIANCE:
pose_covariance = PoseWithCovarianceStamped()
pose_covariance.header = Utils.make_header("map", stamp)
pose_covariance.pose.pose.position.x = pose[0]
pose_covariance.pose.pose.position.y = pose[1]
pose_covariance.pose.pose.orientation = Utils.angle_to_quaternion(
pose[2])
# Copy in the Covariance Matrix, Converting from 3-D to 6-D
temp_covariance = self.covariance_generator(
self.expected_square_pose(), self.square_expected_pose())
for i in range(0, 2):
for j in range(0, 2):
pose_covariance.pose.covariance[6 * i +
j] = temp_covariance[i][j]
pose_covariance.pose.covariance[6 * 5 + 5] = temp_covariance[2][2]
self.pose_covariance_pub.publish(pose_covariance)
# Publishing "map -> laser" TF Transform
if self.TF_MODE == 1:
self.pub_tf.sendTransform(
(pose[0], pose[1], 0),
tf.transformations.quaternion_from_euler(0, 0, pose[2]), stamp,
"laser", "map")
# Publishing "map -> base_link" TF Transform
elif self.TF_MODE == 2:
self.pub_tf.sendTransform(
(pose[0], pose[1], 0),
tf.transformations.quaternion_from_euler(0, 0, pose[2]), stamp,
"base_link", "map")
# Publishing "map -> odom" TF Transform
elif self.TF_MODE == 3:
try:
# "odom -> base" transform
(odom_base_trans, odom_base_rot) = self.tfTL.lookupTransform(
"odom", "base_link", rospy.Time(0))
odom_base_trans_mat = tf.transformations.translation_matrix(
odom_base_trans)
odom_base_rot_mat = tf.transformations.quaternion_matrix(
odom_base_rot)
odom_base_mat = np.dot(odom_base_trans_mat, odom_base_rot_mat)
except (tf.LookupException, tf.ConnectivityException,
tf.ExtrapolationException):
return
# "map -> base" transform
map_base_trans = [pose[0], pose[1], 0]
map_base_rot = tf.transformations.quaternion_from_euler(
0, 0, pose[2])
map_base_trans_mat = tf.transformations.translation_matrix(
map_base_trans)
map_base_rot_mat = tf.transformations.quaternion_matrix(
map_base_rot)
map_base_mat = np.dot(map_base_trans_mat, map_base_rot_mat)
# "base -> map" transform
base_map_mat = tf.transformations.inverse_matrix(map_base_mat)
base_map_trans = tf.transformations.translation_from_matrix(
base_map_mat)
base_map_rot = tf.transformations.quaternion_from_matrix(
base_map_mat)
# "odom -> map" transform
odom_map_mat = np.dot(odom_base_mat, base_map_mat)
# "map -> odom" transform
map_odom_mat = tf.transformations.inverse_matrix(odom_map_mat)
map_odom_trans = tf.transformations.translation_from_matrix(
map_odom_mat)
map_odom_rot = tf.transformations.quaternion_from_matrix(
map_odom_mat)
self.pub_tf.sendTransform(map_odom_trans, map_odom_rot, stamp,
"odom", "map")
else:
print "--------------- WARNING : TF_MODE was wrongly selected ---------------"
#------------------------------------------------------------------------------------------------------------
return # below this line is disabled
"""
Our particle filter provides estimates for the "laser" frame
since that is where our laser range estimates are measure from. Thus,
We want to publish a "map" -> "laser" transform.
However, the car's position is measured with respect to the "base_link"
frame (it is the root of the TF tree). Thus, we should actually define
a "map" -> "base_link" transform as to not break the TF tree.
"""
# Get map -> laser transform.
map_laser_pos = np.array((pose[0], pose[1], 0))
map_laser_rotation = np.array(
tf.transformations.quaternion_from_euler(0, 0, pose[2]))
# Apply laser -> base_link transform to map -> laser transform
# This gives a map -> base_link transform
laser_base_link_offset = (0, 0, 0)
map_laser_pos -= np.dot(
tf.transformations.quaternion_matrix(
tf.transformations.unit_vector(map_laser_rotation))[:3, :3],
laser_base_link_offset).T
# Publish transform
self.pub_tf.sendTransform(map_laser_pos, map_laser_rotation, stamp,
"base_link", "map")
