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