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 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 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 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 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 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 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 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 _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 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 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(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 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 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 get_lsq_triangulation_error_with_noisy_gt(self,observation,epsilon=0.5):
noisy_joints = self.get_noisy_joints()
stamp = noisy_joints.header.stamp
noisy_joints_neighbor = self.get_noisy_joints_neighbor(stamp)
try:
self.joints = np.array(noisy_joints.res).reshape((14,2))
self.joints_prev = self.joints
except:
self.joints = self.joints_prev
print('Unable to find noisy joints')
try:
self.joints_neighbor = np.array(noisy_joints_neighbor.res).reshape((14,2))
self.joints_neighbor_prev = self.joints_neighbor
except:
self.joints_neighbor = self.joints_neighbor_prev
print('Unable to find noisy joints from neighbor')
# Get the camera intrinsics of both cameras
P1 = self.get_cam_intrinsic()
P2 = self.get_neighbor_cam_intrinsic()
# Convert world coordinates to local camera coordinates for both cameras
# We get the camera extrinsics as follows
try:
trans,rot = self.listener.lookupTransform(self.rotors_machine_name+'/xtion_rgb_optical_frame','world', stamp) #target to source frame
self.trans_prev = trans
self.rot_prev = rot
except:
trans = self.trans_prev
rot = self.rot_prev
print('Robot rotation unavailable')
(r,p,y) = tf.transformations.euler_from_quaternion(rot)
H1 = tf.transformations.euler_matrix(r,p,y,axes='sxyz')
H1[0:3,3] = trans
try:
trans,rot = self.listener.lookupTransform(self.rotors_neighbor_name+'/xtion_rgb_optical_frame','world', stamp) #target to source frame
self.trans2_prev = trans
self.rot2_prev = rot
except:
trans = self.trans2_prev
rot = self.rot2_prev
print('Robot neighbor rotation unavailable')
(r,p,y) = tf.transformations.euler_from_quaternion(rot)
H2 = tf.transformations.euler_matrix(r,p,y,axes='sxyz')
H2[0:3,3] = trans
#Concatenate Intrinsic Matrices
intrinsic = np.array((P1[0:3,0:3],P2[0:3,0:3]))
#Concatenate Extrinsic Matrices
extrinsic = np.array((H1,H2))
joints_gt = self.get_joints_gt()
error = 0
self.triangulated_root = PoseArray()
for k in range(len(gt_joints)):
p2d1 = self.joints[k,:]
p2d2 = self.joints_neighbor[k,:]
points_2d = np.array((p2d1,p2d2))
# Solve system of equations using least squares to estimate person position in robot 1 camera frame
estimate_root = self.lstsq_triangulation(intrinsic,extrinsic,points_2d,2)
es_root_cam = Point();es_root_cam.x = estimate_root[0];es_root_cam.y = estimate_root[1];es_root_cam.z = estimate_root[2]
err_cov = PoseWithCovarianceStamped()
#if v>4: #because head, eyes and ears lead to high error for the actor
# if v==5 or v == 6:
p = Pose()
p.position = es_root_cam
self.triangulated_root.poses.append(p)
gt = joints_gt.poses[gt_joints[gt_joints_names[k]]].position
error += (self.get_distance_from_point(gt,es_root_cam))
err_cov.pose.pose.position = es_root_cam
err_cov.pose.covariance[0] = (gt.x - es_root_cam.x)**2
err_cov.pose.covariance[7] = (gt.y - es_root_cam.y)**2
err_cov.pose.covariance[14] = (gt.z - es_root_cam.z)**2
err_cov.header.stamp = rospy.Time()
err_cov.header.frame_id = 'world'
self.triangulated_cov_pub[k].publish(err_cov)
# Publish all estimates
self.triangulated_root.header.stamp = rospy.Time()
self.triangulated_root.header.frame_id = 'world'
self.triangulated_pub.publish(self.triangulated_root)
is_in_desired_pos = False
error = error/len(gt_joints)
if error<=epsilon:
# error = 0.4*error/epsilon
is_in_desired_pos = True
# else:
# error = 0.4
return [is_in_desired_pos,error]
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 yaw_to_quat(yaw):
"""
Computing corresponding quaternion q to angle yaw [rad]
:param yaw
:return: q
"""
q = Quaternion(axis=[0, 0, 1], angle=yaw)
return q.elements
rospy.init_node('init_pos')
pub = rospy.Publisher('/initialpose', PoseWithCovarianceStamped, queue_size=1)
initpose_msg = PoseWithCovarianceStamped()
initpose_msg.header.frame_id = "map"
initpose_msg.pose.pose.position.x = float(sys.argv[1])
initpose_msg.pose.pose.position.y = float(sys.argv[2])
quaternion = yaw_to_quat((float(sys.argv[3])))
initpose_msg.pose.pose.orientation.x = quaternion[0]
initpose_msg.pose.pose.orientation.y = quaternion[1]
initpose_msg.pose.pose.orientation.z = quaternion[2]
initpose_msg.pose.pose.orientation.w = quaternion[3]
rospy.sleep(1)
rospy.loginfo("Setting initial pose")
pub.publish(initpose_msg)
print(initpose_msg)
rospy.loginfo("Initial pose SET")
def execute_overhead_callback(self, goal):
now = rospy.Time.now()
if self.ros_overhead_cam_info is None:
raise Exception("Camera Info not received")
self.last_ros_image_stamp = self.ros_image_stamp
try:
self.ros_overhead_trigger.wait_for_service(timeout=0.1)
self.ros_overhead_trigger()
except:
pass
wait_count = 0
while self.ros_overhead_cam_info.ros_image is None or self.ros_image_stamp == self.last_ros_image_stamp:
if wait_count > 250:
raise Exception("Image receive timeout")
time.sleep(0.25)
wait_count += 1
print "Past timeout"
#stored data in local function variable, so changed all references off of self.ros_image
img = self.ros_overhead_cam_info.ros_image
if img is None:
raise Exception("Camera image data not received")
print "image received"
r_array = RecognizedObjectArray()
r_array.header.stamp = now
r_array.header.frame_id = self.frame_id
if goal.use_roi:
raise Warning("use_roi in ObjectRecognitionRequest ignored")
search_objects = dict()
with self.payloads_lock:
for _, p in self.payloads.items():
search_objects[p.name] = p.markers
for _, l in self.link_markers.items():
search_objects[l.header.frame_id] = l.markers
#TODO: handle multiple aruco dictionaries, currently only use one
print "past payload lock"
aruco_dicts = set()
for m in search_objects.itervalues():
for m2 in m:
aruco_dicts.add(m2.marker.dictionary)
print len(aruco_dicts)
aruco_dicts.add("DICT_ARUCO_ORIGINAL")
assert len(
aruco_dicts
) == 1, "Currently all tags must be from the same dictionary"
if not hasattr(cv2.aruco, next(iter(aruco_dicts))):
raise ValueError("Invalid aruco-dict value")
aruco_dict_id = getattr(cv2.aruco, next(iter(aruco_dicts)))
aruco_dict = cv2.aruco.Dictionary_get(aruco_dict_id)
c_pose = self.listener.lookupTransform(
"/world", self.ros_overhead_cam_info.ros_data.header.frame_id,
rospy.Time(0))
print "c_pose"
print c_pose.p
print c_pose.R
parameters = cv2.aruco.DetectorParameters_create()
parameters.cornerRefinementWinSize = 32
parameters.cornerRefinementMethod = cv2.aruco.CORNER_REFINE_SUBPIX
corners, ids, rejectedImgPoints = cv2.aruco.detectMarkers(
img, aruco_dict, parameters=parameters)
print ids
for object_name, payload_markers in search_objects.items():
tag_object_poses = dict()
for m in payload_markers:
board = get_aruco_gridboard(m.marker, aruco_dict)
#board = cv2.aruco.GridBoard_create(4, 4, .04, .0075, aruco_dict, 16)
retval, rvec, tvec = cv2.aruco.estimatePoseBoard(
corners, ids, board, self.ros_overhead_cam_info.camMatrix,
self.ros_overhead_cam_info.distCoeffs)
print retval
print rvec
print tvec
if (retval > 0):
if (m.name == "leeward_mid_panel_marker_1"
or m.name == "leeward_tip_panel_marker_1"):
print "Found tag: " + m.name
try:
#o_pose = self.listener.lookupTransform(m.name, object_name, rospy.Time(0))
o_pose = rox_msg.msg2transform(m.pose).inv()
print object_name
print o_pose
print ""
print "o_pose"
print o_pose.p
print o_pose.R
Ra, b = cv2.Rodrigues(rvec)
a_pose = rox.Transform(Ra, tvec)
print "a_pose"
print a_pose.p
print a_pose.R
tag_object_poses[m.name] = c_pose * (a_pose *
o_pose)
except (tf.LookupException, tf.ConnectivityException,
tf.ExtrapolationException):
continue
#TODO: merge tag data if multiple tags detected
if len(tag_object_poses) > 0:
p = PoseWithCovarianceStamped()
p.pose.pose = rox_msg.transform2pose_msg(
tag_object_poses.itervalues().next())
p.header.stamp = now
p.header.frame_id = self.frame_id
r = RecognizedObject()
r.header.stamp = now
r.header.frame_id = self.frame_id
r.type.key = object_name
r.confidence = 1
r.pose = p
r_array.objects.append(r)
"""for o in self.object_names:
try:
(trans,rot) = self.listener.lookupTransform("/world", o, rospy.Time(0))
print o
print trans
print rot
print ""
p=PoseWithCovarianceStamped()
p.pose.pose=Pose(Point(trans[0], trans[1], trans[2]), Quaternion(rot[0], rot[1], rot[2], rot[3]))
p.header.stamp=now
p.header.frame_id=self.frame_id
r=RecognizedObject()
r.header.stamp=now
r.header.frame_id=self.frame_id
r.type.key=o
r.confidence=1
r.pose=p
r_array.objects.append(r)
except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
continue"""
print "Succeeded in finding tags"
result = ObjectRecognitionResult()
result.recognized_objects = r_array
self.overhead_server.set_succeeded(result=result)
def __init__(self):
rospy.init_node('exploring_random', anonymous=True)
rospy.on_shutdown(self.shutdown)
# 在每个目标位置暂停的时间 (单位：s)
self.rest_time = rospy.get_param("~rest_time", 2)
# 是否仿真？
self.fake_test = rospy.get_param("~fake_test", True)
# 到达目标的状态
goal_states = [
'PENDING', 'ACTIVE', 'PREEMPTED', 'SUCCEEDED', 'ABORTED',
'REJECTED', 'PREEMPTING', 'RECALLING', 'RECALLED', 'LOST'
]
# 设置目标点的位置
# 在rviz中点击 2D Nav Goal 按键，然后单击地图中一点
# 在终端中就会看到该点的坐标信息
locations = dict()
locations['1'] = Pose(Point(4.589, -0.376, 0.000),
Quaternion(0.000, 0.000, -0.447, 0.894))
locations['2'] = Pose(Point(4.231, -6.050, 0.000),
Quaternion(0.000, 0.000, -0.847, 0.532))
locations['3'] = Pose(Point(-0.674, -5.244, 0.000),
Quaternion(0.000, 0.000, 0.000, 1.000))
locations['4'] = Pose(Point(-5.543, -4.779, 0.000),
Quaternion(0.000, 0.000, 0.645, 0.764))
locations['5'] = Pose(Point(-4.701, -0.590, 0.000),
Quaternion(0.000, 0.000, 0.340, 0.940))
locations['6'] = Pose(Point(2.924, 0.018, 0.000),
Quaternion(0.000, 0.000, 0.000, 1.000))
# 发布控制机器人的消息
self.cmd_vel_pub = rospy.Publisher('cmd_vel', Twist, queue_size=5)
# 订阅move_base服务器的消息
self.move_base = actionlib.SimpleActionClient("move_base",
MoveBaseAction)
rospy.loginfo("Waiting for move_base action server...")
# 60s等待时间限制
self.move_base.wait_for_server(rospy.Duration(60))
rospy.loginfo("Connected to move base server")
# 保存机器人的在rviz中的初始位置
initial_pose = PoseWithCovarianceStamped()
# 保存成功率、运行时间、和距离的变量
n_locations = len(locations)
n_goals = 0
n_successes = 0
i = n_locations
distance_traveled = 0
start_time = rospy.Time.now()
running_time = 0
location = ""
last_location = ""
# 确保有初始位置
while initial_pose.header.stamp == "":
rospy.sleep(1)
rospy.loginfo("Starting navigation test")
# 开始主循环，随机导航
while not rospy.is_shutdown():
# 如果已经走完了所有点，再重新开始排序
if i == n_locations:
i = 0
sequence = sample(locations, n_locations)
# 如果最后一个点和第一个点相同，则跳过
if sequence[0] == last_location:
i = 1
# 在当前的排序中获取下一个目标点
location = sequence[i]
# 跟踪行驶距离
# 使用更新的初始位置
if initial_pose.header.stamp == "":
distance = sqrt(
pow(
locations[location].position.x -
locations[last_location].position.x, 2) + pow(
locations[location].position.y -
locations[last_location].position.y, 2))
else:
rospy.loginfo("Updating current pose.")
distance = sqrt(
pow(
locations[location].position.x -
initial_pose.pose.pose.position.x, 2) + pow(
locations[location].position.y -
initial_pose.pose.pose.position.y, 2))
initial_pose.header.stamp = ""
# 存储上一次的位置，计算距离
last_location = location
# 计数器加1
i += 1
n_goals += 1
# 设定下一个目标点
self.goal = MoveBaseGoal()
self.goal.target_pose.pose = locations[location]
self.goal.target_pose.header.frame_id = 'map'
self.goal.target_pose.header.stamp = rospy.Time.now()
# 让用户知道下一个位置
rospy.loginfo("Going to: " + str(location))
# 向下一个位置进发
self.move_base.send_goal(self.goal)
# 五分钟时间限制
finished_within_time = self.move_base.wait_for_result(
rospy.Duration(300))
# 查看是否成功到达
if not finished_within_time:
self.move_base.cancel_goal()
rospy.loginfo("Timed out achieving goal")
else:
state = self.move_base.get_state()
if state == GoalStatus.SUCCEEDED:
rospy.loginfo("Goal succeeded!")
n_successes += 1
distance_traveled += distance
rospy.loginfo("State:" + str(state))
else:
rospy.loginfo("Goal failed with error code: " +
str(goal_states[state]))
# 运行所用时间
running_time = rospy.Time.now() - start_time
running_time = running_time.secs / 60.0
# 输出本次导航的所有信息
rospy.loginfo("Success so far: " + str(n_successes) + "/" +
str(n_goals) + " = " +
str(100 * n_successes / n_goals) + "%")
rospy.loginfo("Running time: " + str(trunc(running_time, 1)) +
" min Distance: " +
str(trunc(distance_traveled, 1)) + " m")
rospy.sleep(self.rest_time)
def fid_cb(msg):
"""
callback which takes fiducial transfroms and publishes pose of
robot base relative to map
@param msg: A fiducial_msgs FidcucialTransformArray object
"""
global tf_broadcaster
global tf_listener
global FIDUCIAL_NAMES
global pose_pub
# if driving, don't interupt
if get_state() not in [States.LOST, States.TELEOP]: #removed docked state
return
# if fiducials found, take the first
if len(msg.transforms) == 0:
return
transform = msg.transforms[0]
# swap y and z axes to fit map frame of reference
pos = transform.transform.translation
rot = transform.transform.rotation
pos.x, pos.y, pos.z = pos.x, pos.z, pos.y
rot.x, rot.y, rot.z = rot.x, rot.z, rot.y
transform.transform.translation = pos
transform.transform.rotation = rot
# invert the transform
homo_mat = PoseConv().to_homo_mat(transform.transform)
inverted_tf = PoseConv().to_tf_msg(np.linalg.inv(homo_mat))
# send a transform from camera to fiducial
m = TransformStamped()
m.transform = inverted_tf
m.header.frame_id = FIDUCIAL_NAMES.get(str(transform.fiducial_id))
m.header.stamp = rospy.Time.now()
m.child_frame_id = "fiducial_camera"
tf_broadcaser.sendTransform(m)
# calculate transform from map to base
try:
latest_time = tf_listener.getLatestCommonTime("/map","/fiducial_base")
base_to_map = tf_listener.lookupTransform("/map","/fiducial_base",latest_time)
except tf2_ros.TransformException:
rospy.logwarn("failed to transform, is fiducial {} mapped?".format(transform.fiducial_id))
return
# convert transform to PoseWithCovarianceStamped
robot_pose = PoseConv().to_pose_msg(base_to_map)
pose_w_cov_stamped = PoseWithCovarianceStamped()
pose_w_cov_stamped.pose.pose = robot_pose
pose_w_cov_stamped.header.stamp = rospy.Time.now()
pose_w_cov_stamped.header.frame_id = "map"
rospy.logdebug("Sending fiducial pose:\n{}".format(robot_pose))
# publish to /initialpose
pose_pub.publish(pose_w_cov_stamped)
# update state
try:
prev_state = get_state()
if prev_state == States.LOST:
change_state(States.WAITING)
rospy.loginfo("Fiducial {} seen, no longer lost".format(transform.fiducial_id))
except rospy.ServiceException:
rospy.logerr("Could not access state service")
def __init__(self):
rospy.init_node('random_navigation', anonymous=True)
rospy.on_shutdown(self.shutdown)
# 在每个目标位置暂停的时间
self.rest_time = rospy.get_param("~rest_time", 2)
# 到达目标的状态
goal_states = [
'PENDING', 'ACTIVE', 'PREEMPTED', 'SUCCEEDED', 'ABORTED',
'REJECTED', 'PREEMPTING', 'RECALLING', 'RECALLED', 'LOST'
]
# 设置目标点的位置
# 如果想要获得某一点的坐标，在rviz中点击 2D Nav Goal 按键，然后单机地图中一点
# 在终端中就会看到坐标信息
locations = dict()
locations['p1'] = Pose(Point(1.150, 5.461, 0.000),
Quaternion(0.000, 0.000, -0.013, 1.000))
locations['p2'] = Pose(Point(6.388, 2.66, 0.000),
Quaternion(0.000, 0.000, 0.063, 0.998))
locations['p3'] = Pose(Point(8.089, -1.657, 0.000),
Quaternion(0.000, 0.000, 0.946, -0.324))
locations['p4'] = Pose(Point(9.767, 5.171, 0.000),
Quaternion(0.000, 0.000, 0.139, 0.990))
locations['p5'] = Pose(Point(0.502, 1.270, 0.000),
Quaternion(0.000, 0.000, 0.919, -0.392))
locations['p6'] = Pose(Point(4.557, 1.234, 0.000),
Quaternion(0.000, 0.000, 0.627, 0.779))
# 发布控制机器人的消息
self.cmd_vel_pub = rospy.Publisher('cmd_vel', Twist, queue_size=5)
# 订阅move_base服务器的消息
self.move_base = actionlib.SimpleActionClient("move_base",
MoveBaseAction)
rospy.loginfo("Waiting for move_base action server...")
# 60s等待时间限制
self.move_base.wait_for_server(rospy.Duration(60))
rospy.loginfo("Connected to move base server")
# 保存机器人的在rviz中的初始位置
initial_pose = PoseWithCovarianceStamped()
# 保存成功率、运行时间、和距离的变量
n_locations = len(locations)
n_goals = 0
n_successes = 0
i = n_locations
distance_traveled = 0
start_time = rospy.Time.now()
running_time = 0
location = ""
last_location = ""
# 确保有初始位置
while initial_pose.header.stamp == "":
rospy.sleep(1)
rospy.loginfo("Starting navigation test")
# 开始主循环，随机导航
if not rospy.is_shutdown():
# 在当前的排序中获取下一个目标点
location = 'p1'
# 跟踪行驶距离
# 使用更新的初始位置
if initial_pose.header.stamp == "":
distance = sqrt(
pow(
locations[location].position.x -
locations[last_location].position.x, 2) + pow(
locations[location].position.y -
locations[last_location].position.y, 2))
else:
rospy.loginfo("Updating current pose.")
distance = sqrt(
pow(
locations[location].position.x -
initial_pose.pose.pose.position.x, 2) + pow(
locations[location].position.y -
initial_pose.pose.pose.position.y, 2))
initial_pose.header.stamp = ""
# 存储上一次的位置，计算距离
last_location = location
# 计数器加1
i += 1
n_goals += 1
# 设定下一个目标点
self.goal = MoveBaseGoal()
self.goal.target_pose.pose = locations[location]
self.goal.target_pose.header.frame_id = 'map'
self.goal.target_pose.header.stamp = rospy.Time.now()
# 让用户知道下一个位置
rospy.loginfo("Going to: " + str(location))
# 向下一个位置进发
self.move_base.send_goal(self.goal)
# 五分钟时间限制
finished_within_time = self.move_base.wait_for_result(
rospy.Duration(300))
# 查看是否成功到达
if not finished_within_time:
self.move_base.cancel_goal()
rospy.loginfo("Timed out achieving goal")
else:
state = self.move_base.get_state()
if state == GoalStatus.SUCCEEDED:
rospy.loginfo("Goal succeeded!")
n_successes += 1
distance_traveled += distance
rospy.loginfo("State:" + str(state))
else:
rospy.loginfo("Goal failed with error code: " +
str(goal_states[state]))
# 运行所用时间
running_time = rospy.Time.now() - start_time
running_time = running_time.secs / 60.0
# 输出本次导航的所有信息
rospy.loginfo("Success so far: " + str(n_successes) + "/" +
str(n_goals) + " = " +
str(100 * n_successes / n_goals) + "%")
rospy.loginfo("Running time: " + str(trunc(running_time, 1)) +
" min Distance: " +
str(trunc(distance_traveled, 1)) + " m")
rospy.sleep(self.rest_time)
def __init__(self):
rospy.init_node('nav_test', anonymous=True)
rospy.on_shutdown(self.shutdown)
# How long in seconds should the robot pause at each location?
self.rest_time = rospy.get_param("~rest_time", 2)
# Are we running in the fake simulator?
self.fake_test = rospy.get_param("~fake_test", False)
# Goal state return values
goal_states = ['PENDING', 'ACTIVE', 'PREEMPTED',
'SUCCEEDED', 'ABORTED', 'REJECTED',
'PREEMPTING', 'RECALLING', 'RECALLED',
'LOST']
locations = dict()
locations['A'] = Pose(Point(1.279, -0.265, 0.000), Quaternion(0.000, 0.000, 0.318, 0.948))
locations['厨房'] = Pose(Point(2.539, 1.657, 0.000), Quaternion(00.000, 0.000, 0.000, 1.000))
locations['卧室'] = Pose(Point(1.337, 1.090, 0.000), Quaternion(0.000, 0.000, 0.924, 0.382))
locations['D'] = Pose(Point(-0.324, -0.276, 0.000), Quaternion(0.000, 0.000, 0.000, 1.000))
self.cmd_vel_pub = rospy.Publisher('cmd_vel', Twist, queue_size=5)
# Subscribe to the move_base action server
self.move_base = actionlib.SimpleActionClient("move_base", MoveBaseAction)
rospy.loginfo("Waiting for move_base action server...")
# Wait 60 seconds for the action server to become available
self.move_base.wait_for_server(rospy.Duration(60))
rospy.loginfo("Connected to move base server")
# A variable to hold the initial pose of the robot to be set by
# the user in RViz
initial_pose = PoseWithCovarianceStamped()
# Variables to keep track of success rate, running time,
# and distance traveled
n_locations = len(locations)
n_goals = 0
n_successes = 0
i = n_locations
distance_traveled = 0
start_time = rospy.Time.now()
running_time = 0
location = ""
last_location = ""
self.Dist = 0
self.Asr = ""
Loc_1=0
Loc_2=0
Name_1=0
name_2=0
Name=""
Drink_1=0
Drink_2=0
Drink_3=0
Drink=""
# Get the initial pose from the user (how to set)
rospy.loginfo("*** Click the 2D Pose Estimate button in RViz to set the robot's initial pose...")
rospy.wait_for_message('initialpose', PoseWithCovarianceStamped)
self.last_location = Pose()
rospy.Subscriber('initialpose', PoseWithCovarianceStamped, self.update_initial_pose)
# Make sure we have the initial pose
while initial_pose.header.stamp == "":
rospy.sleep(1)
rospy.loginfo("Starting navigation test")
dict_keys = ['A','厨房','D','客厅']
pub1 = rospy.Publisher('/voice/xf_tts_topic', String, queue_size=5)
pub2 = rospy.Publisher('/voice/xf_asr_topic', Int32, queue_size=5)
pub3 = rospy.Publisher('arm',String,queue_size=5)
pub4 = rospy.Publisher('tracking',Int32,queue_size=5)
pub5 = rospy.Publisher('/following',Int32,queue_size=5)
pub6 = rospy.Publisher('drink_type',String,queue_size=5)
pub7 = rospy.Publisher('ros2_wake',Int32,queue_size=5)
#######################################
# Begin the main loop and run through a sequence of locations
while not rospy.is_shutdown():
if i == n_locations:
i = 0
# sequence = sample(locations, n_locations)
# Skip over first location if it is the same as
# the last location
if dict_keys[0] == last_location:
i = 1
#####################################
####### Modify by XF 2017.4.14 ######
# location = sequence[i]
# rospy.loginfo("location= " + str(location))
# location = locations.keys()[i]
location = dict_keys[i]
#####################################
# Keep track of the distance traveled.
# Use updated initial pose if available.
if initial_pose.header.stamp == "":
distance = sqrt(pow(locations[location].position.x -
locations[last_location].position.x, 2) +
pow(locations[location].position.y -
locations[last_location].position.y, 2))
else:
rospy.loginfo("Updating current pose.")
distance = sqrt(pow(locations[location].position.x -
initial_pose.pose.pose.position.x, 2) +
pow(locations[location].position.y -
initial_pose.pose.pose.position.y, 2))
initial_pose.header.stamp = ""
# Store the last location for distance calculations
last_location = location
# Increment the counters
i += 1
n_goals += 1
# Set up the next goal location
self.goal = MoveBaseGoal()
self.goal.target_pose.pose = locations[location]
self.goal.target_pose.header.frame_id = 'map'
self.goal.target_pose.header.stamp = rospy.Time.now()
# Let the user know where the robot is going next
rospy.loginfo("Going to: " + str(location))
# Start the robot toward the next location
self.move_base.send_goal(self.goal)
# Allow 6 minutes to get there
finished_within_time = self.move_base.wait_for_result(rospy.Duration(360))
# Check for success or failure
if not finished_within_time:
self.move_base.cancel_goal()
rospy.loginfo("Timed out achieving goal")
else:
state = self.move_base.get_state()
if state == GoalStatus.ABORTED: # can not find a plan,give feedback
rospy.loginfo("please get out")
status_n = "please get out"
pub1.publish(status_n)
if state == GoalStatus.SUCCEEDED:
rospy.loginfo("Goal succeeded!")
n_successes += 1
distance_traveled += distance
rospy.loginfo("State:" + str(state))
############ add voice by XF 4.25 ################## The 1st
# pub1 = rospy.Publisher('/voice/xf_tts_topic', String, queue_size=5)
loc ="我已到达"+str(location)
pub1.publish(loc)
rospy.sleep(5)
rospy.Subscriber('dist',Int32,self.callbackDist) #now robot already adjust correctly
rospy.loginfo("Dist:"+str(self.Dist))
if str(location) == 'A':
commandA1="您好，有什么可以帮助您?"
pub1.publish(commandA1)
############# now next asr ######################
awake=1
pub2.publish(awake)
rospy.Subscriber('/voice/xf_asr_follow',String,self.callbackAsr)
###### Modify by XF 2017. 4.21 #########
rospy.sleep(15)
rospy.loginfo("self.Asr:"+str(self.Asr))
Loc_1=string.find(self.Asr,'卧室')
Loc_2=string.find(self.Asr,'客厅')
Name_1=string.find(self.Asr,'张三')
Name_2=string.find(self.Asr,'李四')
Drink_1=string.find(self.Asr,'可乐')
Drink_2=string.find(self.Asr,'雪碧')
Drink_3=string.find(self.Asr,'橙汁')
if Loc_1!=-1:
rospy.loginfo("匹配卧室")
dict_keys = ['A','厨房','卧室','D']
if Loc_2!=-1:
rospy.loginfo("匹配客厅")
dict_keys = ['A','厨房','客厅','D']
if Name_1!=-1:
Name = '张三'
rospy.loginfo("匹配张三")
if Name_2!=-1:
Name = '李四'
rospy.loginfo("匹配李四")
if Drink_1!=-1:
Drink = '可乐'
if Drink_2!=-1:
Drink = '雪碧'
if Drink_3!=-1:
Drink = '橙汁'
ros2_wake=1
pub7.publish(ros2_wake)
pub6.publish(Drink)
#######################################
rospy.sleep(5)
commandA2="我已获取任务，请稍等."
pub1.publish(commandA2)
################################################## The 2nd
if str(location) == '厨房':
############## add by XF 5.27 ##################
rospy.sleep(5)
track=1
pub4.publish(track)
rospy.sleep(10)
rospy.Subscriber('dist',Int32,self.callbackDist) #now robot already adjust correctly
rospy.loginfo("Dist2:"+str(self.Dist))
rospy.loginfo("go away Dist:"+str(self.Dist/1000.0-0.3))
self.goal = MoveBaseGoal()
self.goal.target_pose.pose.position.x = self.Dist/1000.0-0.3
self.goal.target_pose.pose.orientation.w = 1.0;
self.goal.target_pose.header.frame_id = 'base_footprint'
self.goal.target_pose.header.stamp = rospy.Time.now()
self.move_base.send_goal(self.goal)
rospy.sleep(5)
################################################
commandB1="pick"
pub3.publish(commandB1)
rospy.sleep(10)
commandB2="我已找到"+str(Drink)+"并抓取."
pub1.publish(commandB2)
################################################## The 3rd
if str(location) == '卧室':
# rospy.sleep(5)
follow=1
pub5.publish(follow)
track=2 #stop tracking
pub4.publish(track)
rospy.loginfo("send stop tracking command")
#subscribe the angle
# rospy.Subscriber('xf_xfm_node', Int32, self.callbackXFM)
rospy.sleep(10)
commandC2=str(Name)+"给您"+str(Drink)
pub1.publish(commandC2)
rospy.sleep(3)
commandC1="release"
pub3.publish(commandC1)
rospy.sleep(50)
##################################################
else:
rospy.loginfo("Goal failed with error code: " + str(goal_states[state]))
# How long have we been running?
running_time = rospy.Time.now() - start_time
running_time = running_time.secs / 60.0
# Print a summary success/failure, distance traveled and time elapsed
rospy.loginfo("Success so far: " + str(n_successes) + "/" +
str(n_goals) + " = " +
str(100 * n_successes/n_goals) + "%")
rospy.loginfo("Running time: " + str(trunc(running_time, 1)) +
" min Distance: " + str(trunc(distance_traveled, 1)) + " m")
rospy.sleep(self.rest_time)
def futureSim(passed_roomba_state, passed_actions, start_pose):
"""
This simulation is for short term rapid location estimation of a Roomba that hasn't been seen for a while.
20 hypothetical "Imaginary Roombas" trace out paths it could have taken,
and the centroid of the final scatter determines the hypothesized final position of the actual Roomba.
times_since = Array. Time since the Roomba was last seen.
:arg (rospy.Time) last_turn: The last time the Roomba turned every 20 seconds
:arg (rospy.Time) last_noise: The last time the Roomba had noise
:arg (PoseWithCovarianceStamped) start_pose: The start pose of the Roomba
:arg (rospy.Time) end_time: When we want to simulate until
:returns prediction: The estimated final position
:rtype: PoseWithCovarianceStamped
"""
(last_turn, last_noise, end_time) = passed_roomba_state
roomba_pos = [(0, 0)] * 20 # Generate 20 hypothetical Roombas' positions
known_time = start_pose.header.stamp
sim_duration = (end_time - known_time).to_sec()
start_pos = start_pose.pose.pose.position
quaternion = start_pose.pose.pose.orientation
quaternion_arr = [quaternion.x, quaternion.y, quaternion.z, quaternion.w]
start_orient = tf.transformations.euler_from_quaternion(quaternion_arr)[2]
# Check time remaining until Roomba experiences noise
time_to_noise = 5 - (known_time - last_noise).to_sec() % 5
# Check number of times Roombas should go through "noise"
noise_cycles = int(math.ceil((sim_duration - time_to_noise) / 5.0))
# Check time remaining until Roomba turns
time_to_turn = 20 - (known_time - last_turn).to_sec() % 20
# Check number of times Roombas should go through "turn cycles"
turn_cycles = int(math.ceil((sim_duration - time_to_turn) / 20.0))
event_queue = populateQueue(passed_actions,(turn_cycles,time_to_turn),(noise_cycles,time_to_noise))
# For one of 20 hypothetical roombas...
for roomba in range(0, len(roomba_pos)):
print "Roomba %i" % roomba
orient = start_orient
prev_time = 0.0
roomba_pos[roomba] = start_pos.x, start_pos.y, orient
for item in event_queue:
(event, time_to_event) = item
assert (time_to_event >= prev_time) or ((event is Action.TURN) or (event is Action.NOISE) or (event is Action.COLLISION)), \
"Event time error. %r occurs too rapidly after previous event." % (event)
if time_to_event - prev_time >= 0: roomba_pos[roomba] = locCalc(roomba_pos[roomba], time_to_event - prev_time, orient)
(orient, event_length) = performEvent(orient, event)
roomba_pos[roomba] = locCalc(roomba_pos[roomba], 0.0, orient)
prev_time = time_to_event + event_length
# rospy.loginfo_throttle("Simulated positions: {}".format(roomba_pos))
mean_pos = np.mean(roomba_pos, axis=0)
# print mean_pos
mean_x = mean_pos[0]
mean_y = mean_pos[1]
mean_quaternion = tf.transformations.quaternion_from_euler(0, 0, mean_pos[2])
msg = Pose(position=Point(mean_x, mean_y, 0), orientation=Quaternion(mean_quaternion[0], mean_quaternion[1], mean_quaternion[2], mean_quaternion[3]))
# TODO: Calculate variance using eigenvectors of covariance matrix
pad = np.zeros(20)
XYZRPY = [np.array(roomba_pos)[:,0],np.array(roomba_pos)[:,1],pad,pad,pad,np.array(roomba_pos)[:,2]]
cov = np.cov(XYZRPY)
print cov
# eigvals, eigvecs = np.linalg.eig(covariance)
# print "Eigenvalues", eigvals
# print "Eigenvectors", eigvecs
return PoseWithCovarianceStamped(
header=start_pose.header,
pose=PoseWithCovariance(
pose=msg,
covariance=cov
)
)
#!/usr/bin/env python
import rospy
from geometry_msgs.msg import PoseWithCovarianceStamped
import time
if __name__ == "__main__":
rospy.init_node("initial_pose_publisher_script")
pub = rospy.Publisher("/initialpose",
PoseWithCovarianceStamped,
queue_size=10)
print("Created publisher.")
rospy.sleep(1.0)
msg = PoseWithCovarianceStamped()
msg.pose.pose.position.x = 71.86
msg.pose.pose.position.y = 97.99
pub.publish(msg)
print("Published message.")
rospy.spin()
def __init__(self):
# Give the node a name
rospy.init_node('ccam_navigation', anonymous=True)
# Set rospy to execute a shutdown function when exiting
rospy.on_shutdown(self.shutdown)
# The minimum time the robot should pause at each location
self.rest_time = rospy.get_param("~rest_time", 5)
# Goal state return values
goal_states = ['PENDING', 'ACTIVE', 'PREEMPTED',
'SUCCEEDED', 'ABORTED', 'REJECTED',
'PREEMPTING', 'RECALLING', 'RECALLED',
'LOST']
# Set up the goal locations. Poses are defined in the map frame.
# An easy way to find the pose coordinates is to point-and-click
# Nav Goals in RViz when running in the simulator.
# Pose coordinates are then displayed in the terminal
# that was used to launch RViz.
locations = dict()
locations['start'] = Pose(Point(0.000, 0.000, 0.000), Quaternion(0.000, 0.000, 0.000, 1.000))
locations['5m'] = Pose(Point(5.000, 0.000, 0.000), Quaternion(0.000, 0.000, 0.000, 1.000))
locations['10m'] = Pose(Point(10.000, -0.525, 0.000), Quaternion(0.000, 0.000, 0.000, 1.000))
locations['20m'] = Pose(Point(20.000, -1.000, 0.000), Quaternion(0.000, 0.000, 0.000, 1.000))
locations['40m'] = Pose(Point(40.000, -1.250, 0.000), Quaternion(0.000, 0.000, 1.000, 0.000))
locations['drill_start'] = Pose(Point(0.400, 0.000, 0.000), Quaternion(0.000, 0.000, -0.700, 0.700))
locations['seal_start'] = Pose(Point(-0.600, 0.000, 0.000), Quaternion(0.000, 0.000, 0.700, 0.700))
# Publisher to manually control the robot (e.g. to stop it, queue_size=5)
self.cmd_vel_pub = rospy.Publisher('cmd_vel', Twist, queue_size=5)
# Subscribe to the move_base action server
self.move_base = actionlib.SimpleActionClient("move_base", MoveBaseAction)
rospy.loginfo("Waiting for move_base action server...")
# Wait 60 seconds for the action server to become available
self.move_base.wait_for_server(rospy.Duration(60))
rospy.loginfo("Connected to move base server")
# A variable to hold the initial pose of the robot to be set by
# the user in RViz
initial_pose = PoseWithCovarianceStamped()
# Get the initial pose from the user
rospy.loginfo("*** Click the 2D Pose Estimate button in RViz to set the robot's initial pose...")
rospy.wait_for_message('initialpose', PoseWithCovarianceStamped)
rospy.Subscriber('initialpose', PoseWithCovarianceStamped, self.update_initial_pose)
# Make sure we have the initial pose
while initial_pose.header.stamp == "":
rospy.sleep(1)
rospy.loginfo("Starting navigation test")
# Begin the main loop and navigation to user specified locations
while not rospy.is_shutdown():
location = raw_input('Enter desired location (ie. start, drill_station_1, etc.): ')
if location == "start" or location == "5m" or location == "10m" or location == "20m" or location == "40m" or location == "drill_start" or location == "seal_start":
# Set up the goal location
self.goal = MoveBaseGoal()
self.goal.target_pose.pose = locations[location]
self.goal.target_pose.header.frame_id = 'map'
self.goal.target_pose.header.stamp = rospy.Time.now()
# Let the user know where the robot is going next
rospy.loginfo("Going to: " + str(location))
# Start the robot toward user specified location
self.move_base.send_goal(self.goal)
# Allow 5 minutes to get there
finished_within_time = self.move_base.wait_for_result(rospy.Duration(300))
# Check for success or failure
if not finished_within_time:
self.move_base.cancel_goal()
rospy.loginfo("Timed out achieving goal")
else:
state = self.move_base.get_state()
if state == GoalStatus.SUCCEEDED:
rospy.loginfo("Reached " + str(location))
rospy.loginfo("State:" + str(state))
else:
rospy.loginfo("Goal failed with error code: " + str(goal_states[state]))
rospy.sleep(self.rest_time)
else:
print "Unknown location selected. Please select a know area."
#!/usr/bin/env python
import rospy
from goal_publisher.msg import PointArray
import actionlib
from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal, MoveBaseResult
from nav_msgs.srv import GetPlan, GetPlanRequest, GetPlanResponse
import time
from geometry_msgs.msg import PoseStamped, PoseWithCovarianceStamped
import numpy as np
from operator import itemgetter
rospy.init_node('goal_navigation')
reach = list()
pos_goal = list()
pos_curr = PoseWithCovarianceStamped()
"""
Getting current position.
"""
def amcl_mycallback(msg):
global pos_curr
pos_curr = msg
"""Sorting goal positions in descending order. Note that, by changing the reverse = false , goals can be sorted
in ascending order. """
def goal_sorting(point):
def __init__(self):
rospy.on_shutdown(self.cleanup)
# Subscribe to the move_base action server
self.move_base = actionlib.SimpleActionClient("move_base", MoveBaseAction)
rospy.loginfo("Waiting for move_base action server...")
pub.publish("Waiting for move_base action server...")
# Wait for the action server to become available
self.move_base.wait_for_server(rospy.Duration(120))
rospy.loginfo("Connected to move base server")
pub.publish("Connected to move base server")
# A variable to hold the initial pose of the robot to be set by the user in RViz
initial_pose = PoseWithCovarianceStamped()
rospy.Subscriber('initialpose', PoseWithCovarianceStamped, self.update_initial_pose)
# Get the initial pose from the user
rospy.loginfo("Click the 2D Pose Estimate button in RViz to set the robot's initial pose")
pub.publish("Click the 2D Pose Estimate button in RViz to set the robot's initial pose")
rospy.wait_for_message('initialpose', PoseWithCovarianceStamped)
# Make sure we have the initial pose
while initial_pose.header.stamp == "":
rospy.sleep(1)
point_pose = String()
rospy.Subscriber('lm_data', String, self.update_point_pose)
while True:
rospy.loginfo("Waiting for your order...")
pub.publish("Waiting for your order...")
rospy.sleep(3)
rospy.wait_for_message('lm_data', String)
if(self.point_pose.data == "GO TO POINT A"):
A_x = 1.8
A_y = -5.51
A_theta = 1.5708
elif(self.point_pose.data == "GO TO POINT B"):
B_x = -2.2
B_y = -2.53
B_theta = 1.5708
elif(self.point_pose.data == "GO TO POINT C"):
C_x = 1.9
C_y = 1.18
C_theta = 1.5708
rospy.loginfo("Ready to go")
pub.publish("Ready to go")
rospy.sleep(1)
locations = dict()
quaternion = quaternion_from_euler(0.0, 0.0, A_theta)
locations['A'] = Pose(Point(A_x, A_y, 0.000), Quaternion(quaternion[0], quaternion[1], quaternion[2], quaternion[3]))
self.goal = MoveBaseGoal()
rospy.loginfo("Starting navigation test")
self.goal.target_pose.header.frame_id = 'map'
self.goal.target_pose.header.stamp = rospy.Time.now()
rospy.loginfo("Going to point "+A_name)
pub.publish("Going to point "+A_name)
rospy.sleep(2)
self.goal.target_pose.pose = locations['A']
self.move_base.send_goal(self.goal)
waiting = self.move_base.wait_for_result(rospy.Duration(300))
if waiting == 1:
rospy.loginfo("Reached point "+A_name)
pub.publish("Reached point "+A_name)
rospy.sleep(2)
def __init__(self):
rospy.init_node('nav_test', anonymous=True)
rospy.on_shutdown(self.shutdown)
# How long in seconds should the robot pause at each location?
self.rest_time = rospy.get_param("~rest_time", 2)
# Are we running in the fake simulator?
self.fake_test = rospy.get_param("~fake_test", False)
# Goal state return values
goal_states = ['PENDING', 'ACTIVE', 'PREEMPTED',
'SUCCEEDED', 'ABORTED', 'REJECTED',
'PREEMPTING', 'RECALLING', 'RECALLED',
'LOST']
# Set up the goal locations. Poses are defined in the map frame.
# An easy way to find the pose coordinates is to point-and-click
# Nav Goals in RViz when running in the simulator.
# Pose coordinates are then displayed in the terminal
# that was used to launch RViz.
locations = dict()
locations['1'] = Pose(Point(45,600, -12,000, 0,000), Quaternion(0,000, 0,000, 0,000, 1,000))
locations['2'] = Pose(Point(60,500, -12,000, 0,000), Quaternion(0,000, 0,000, -0,700, 0,714))
locations['3'] = Pose(Point(60,500, -17,000, 0,000), Quaternion(0,000, 0,000, 1,000, 0,000))
locations['4'] = Pose(Point(45,600, -17,000, 0,000), Quaternion(0,000, 0,000, 0,710, 0,704))
locations['5'] = Pose(Point(45,600, -12,500, 0,000), Quaternion(0,000, 0,000, -0,010, 1,000))
locations['6'] = Pose(Point(60,500, -12,500, 0,000), Quaternion(0,000, 0,000, -0,715, 0,699))
locations['7'] = Pose(Point(60,500, -13,500, 0,000), Quaternion(0,000, 0,000, 1,000, -0,007))
locations['8'] = Pose(Point(45,600, -13,500, 0,000), Quaternion(0,000, 0,000, -0,710, 0,704))
locations['9'] = Pose(Point(45,600, -14,500, 0,000), Quaternion(0,000, 0,000, -0,003, 1,000))
locations['10'] = Pose(Point(60,500, -14,500, 0,000), Quaternion(0,000, 0,000, -0,705, 0,710))
locations['11'] = Pose(Point(60,500, -15,500, 0,000), Quaternion(0,000, 0,000, 1,000, 0,002))
locations['12'] = Pose(Point(45,600, -15,500, 0,000), Quaternion(0,000, 0,000, -0,711, 0,704))
locations['13'] = Pose(Point(45,600, -16,500, 0,000), Quaternion(0,000, 0,000, -0,019, 1,000))
locations['14'] = Pose(Point(60,500, -16,500, 0,000), Quaternion(0,000, 0,000, -0,714, 0,701))
locations['15'] = Pose(Point(60,500, -17,000, 0,000), Quaternion(0,000, 0,000, 1,000, 0,002))
locations['16'] = Pose(Point(45,600, -17,000, 0,000), Quaternion(0,000, 0,000, 0,720, 0,693))
locations['17'] = Pose(Point(45,600, -12,000, 0,000), Quaternion(0,000, 0,000, 0,000, 1,000))
#locations['midten'] = Pose(Point(-42.651, 24.841, 0.000), Quaternion(0.000, 0.000,-0.734, 0.679))
#locations['dining_room_1'] = Pose(Point(-0.861, -0.019, 0.000), Quaternion(0.000, 0.000, 0.892, -0.451))
# Publisher to manually control the robot (e.g. to stop it)
self.cmd_vel_pub = rospy.Publisher('cmd_vel', Twist)
# Subscribe to the move_base action server
self.move_base = actionlib.SimpleActionClient("move_base", MoveBaseAction)
rospy.loginfo("Waiting for move_base action server...")
# Wait 60 seconds for the action server to become available
self.move_base.wait_for_server(rospy.Duration(60))
rospy.loginfo("Connected to move base server")
# A variable to hold the initial pose of the robot to be set by
# the user in RViz
initial_pose = PoseWithCovarianceStamped()
# Variables to keep track of success rate, running time,
# and distance traveled
n_locations = len(locations)
n_goals = 0
n_successes = 0
i = n_locations
distance_traveled = 0
start_time = rospy.Time.now()
running_time = 0
location = ""
last_location = ""
# Get the initial pose from the user
#rospy.loginfo("*** Click the 2D Pose Estimate button in RViz to set the robot's initial pose...")
#rospy.wait_for_message('initialpose', PoseWithCovarianceStamped)
#self.last_location = Pose()
#rospy.Subscriber('initialpose', PoseWithCovarianceStamped, self.update_initial_pose)
# Make sure we have the initial pose
#while initial_pose.header.stamp == "":
# rospy.sleep(1)
rospy.loginfo("Starting navigation test")
# Begin the main loop and run through a sequence of locations
while not rospy.is_shutdown():
# If we've gone through the current sequence,
# start with a new random sequence
if i == n_locations:
i = 0
sequence = ['1','2','3','4','5','6','7','8','9','10','11','12','13','14','15','16','17']
# Skip over first location if it is the same as
# the last location
if sequence[0] == last_location:
i = 1
print sequence
# Get the next location in the current sequence
location = sequence[i]
# Keep track of the distance traveled.
# Use updated initial pose if available.
if initial_pose.header.stamp == "":
distance = sqrt(pow(locations[location].position.x -
locations[last_location].position.x, 2) +
pow(locations[location].position.y -
locations[last_location].position.y, 2))
else:
rospy.loginfo("Updating current pose.")
distance = sqrt(pow(locations[location].position.x -
initial_pose.pose.pose.position.x, 2) +
pow(locations[location].position.y -
initial_pose.pose.pose.position.y, 2))
initial_pose.header.stamp = ""
# Store the last location for distance calculations
last_location = location
# Increment the counters
i += 1
n_goals += 1
# Set up the next goal location
self.goal = MoveBaseGoal()
self.goal.target_pose.pose = locations[location]
self.goal.target_pose.header.frame_id = 'map'
self.goal.target_pose.header.stamp = rospy.Time.now()
# Let the user know where the robot is going next
rospy.loginfo("Going to: " + str(location))
# Start the robot toward the next location
self.move_base.send_goal(self.goal)
# Allow 5 minutes to get there
finished_within_time = self.move_base.wait_for_result(rospy.Duration(300))
# Check for success or failure
if not finished_within_time:
self.move_base.cancel_goal()
rospy.loginfo("Timed out achieving goal")
else:
state = self.move_base.get_state()
if state == GoalStatus.SUCCEEDED:
rospy.loginfo("Goal succeeded!")
n_successes += 1
distance_traveled += distance
rospy.loginfo("State:" + str(state))
else:
rospy.loginfo("Goal failed with error code: " + str(goal_states[state]))
# How long have we been running?
running_time = rospy.Time.now() - start_time
running_time = running_time.secs / 60.0
# Print a summary success/failure, distance traveled and time elapsed
rospy.loginfo("Success so far: " + str(n_successes) + "/" +
str(n_goals) + " = " +
str(100 * n_successes/n_goals) + "%")
rospy.loginfo("Running time: " + str(trunc(running_time, 1)) +
" min Distance: " + str(trunc(distance_traveled, 1)) + " m")
rospy.sleep(self.rest_time)
def _get_obs(self):
"""
Read robot state
:return:
"""
rospy.logdebug("Start Get Observation ==>")
# Pose of the MAV
pose = self.get_gt_odom()
stamp = pose.header.stamp
# Velocity of the MAV
vel = self.get_velocity(stamp)
# Position of the Target
target = self.get_gt_target(stamp)
###################################################################################################
'''
Inputs to Value Network
'''
###################################################################################################
'''Translation of Actor Joints w.r.t world'''
gt = []
for k in range(len(gt_joints)):
try:
(trans,rot) = self.listener.lookupTransform( 'world_ENU',gt_joints_names[k], stamp)
gt.extend([trans[0],trans[1],trans[2]])
except:
(trans,rot) = self.listener.lookupTransform( 'world_ENU',gt_joints_names[k], rospy.Time(0))
gt.extend([trans[0],trans[1],trans[2]])
'''Actor Root Orientation in World frame: Input to Value Function '''
try:
(trans,rot) = self.listener.lookupTransform('world_ENU','actor::Head', stamp)
except:
(trans,rot) = self.listener.lookupTransform('world_ENU','actor::Head', rospy.Time(0))
(r,p,y) = tf.transformations.euler_from_quaternion(rot)
target_orient = y
'''MAV Orientation in World frame: Input to Value Function '''
try:
(trans,rot) = self.listener.lookupTransform( 'world_ENU',self.rotors_machine_name+'/base_link', stamp)
self.yaw1rot_prev = rot
except:
(trans,rot) = self.listener.lookupTransform( 'world_ENU',self.rotors_machine_name+'/base_link', rospy.Time(0))
print('Unable to find yaw1')
(r,p,y) = tf.transformations.euler_from_quaternion(rot)
yaw1 = y
###################################################################################################
###################################################################################################
'''
Inputs for Policy Network
'''
###################################################################################################
'''Actor Root in Mav base link frame: Only Translation. Also used for calculating MAV bearing'''
try:
(trans,rot) = self.listener.lookupTransform(self.rotors_machine_name+'/base_link','actor::actor_pose', stamp)
self.target1_prev = trans
self.target1rot_prev = rot
except:
try:
trans=self.target1_prev
rot=self.target1rot_prev
except:
(trans,rot) = self.listener.lookupTransform(self.rotors_machine_name+'/base_link','actor::actor_pose', rospy.Time(0))
print('Unable to find target:'+self.rotors_machine_name)
theta1 = np.arctan2(trans[1],trans[0])
(r,p,y) = tf.transformations.euler_from_quaternion(rot)
delta_target1 = PoseWithCovarianceStamped();
delta_target1.pose.pose.position.x = trans[0];delta_target1.pose.pose.position.y = trans[1];delta_target1.pose.pose.position.z = trans[2]
'''Actor Root Orientation in Mav base link frame: Only Rotation '''
try:
(trans,rot) = self.listener.lookupTransform(self.rotors_machine_name+'/base_link','actor::Head', stamp)
except:
(trans,rot) = self.listener.lookupTransform(self.rotors_machine_name+'/base_link','actor::Head', rospy.Time(0))
(r,p,y) = tf.transformations.euler_from_quaternion(rot)
delta_orient = y
'''Calculate relative velocity vector in the MAV base link frame'''
homogeneous_matrix = tf.transformations.euler_matrix(0,0,yaw1,axes='sxyz')
homogeneous_matrix[0:3,3] = [0,0,0]
relative_vel = np.dot(np.linalg.inv(homogeneous_matrix),
np.array([vel.twist.twist.linear.y - target.twist.twist.linear.x,
vel.twist.twist.linear.x - target.twist.twist.linear.y,\
-vel.twist.twist.linear.z - target.twist.twist.linear.z,1]))
#Dummy zero inputs to value network during testing.
value_input = [0]*len(list(gt)+[pose.position.x,pose.position.y,pose.position.z,target.twist.twist.linear.x,target.twist.twist.linear.y,target.twist.twist.linear.z,vel.twist.twist.linear.x,vel.twist.twist.linear.y,-vel.twist.twist.linear.z,np.cos(yaw1), np.sin(yaw1),np.cos(target_orient), np.sin(target_orient)])
scale = 2
# Policy Inputs + Value Inputs
observations = [scale*delta_target1.pose.pose.position.x, scale*delta_target1.pose.pose.position.y, scale*delta_target1.pose.pose.position.z,\
scale*relative_vel[0],scale*relative_vel[1],scale*relative_vel[2],np.cos(theta1), np.sin(theta1),np.cos(delta_orient), np.sin(delta_orient)]+value_input
rospy.loginfo("Observations==>"+str(observations))
rospy.loginfo("END Get Observation ==>")
return observations
abs(initial_position.pose.pose.orientation.w - current_pose.pose.pose.orientation.w) < margin ):
return True
else:
return False
# ---------------------------------------------------------------------------- #
# Setup Node and variables #
# ---------------------------------------------------------------------------- #
rospy.init_node('Initial_pose', anonymous=False)
rospy.Subscriber('/odom', Odometry, current_pose_callback)
pub = rospy.Publisher('/initialpose', PoseWithCovarianceStamped, queue_size = 10)
rate = rospy.Rate(1)
current_pose = PoseWithCovarianceStamped()
checkpoint = PoseWithCovarianceStamped()
margin = 0.2
# ---------------------------------------------------------------------------- #
# Initialize setpoint #
# ---------------------------------------------------------------------------- #
checkpoint.pose.pose.position.x = 0.0#-2.0
checkpoint.pose.pose.position.y = 0.0#-0.5
checkpoint.pose.pose.position.z = 0.0
[x,y,z,w]=quaternion_from_euler(0.0,0.0,0.0)
checkpoint.pose.pose.orientation.x = x
checkpoint.pose.pose.orientation.y = y
checkpoint.pose.pose.orientation.z = z
checkpoint.pose.pose.orientation.w = w
def __init__(self):
rospy.init_node('position_nav_node', anonymous=True)
rospy.on_shutdown(self.shutdown)
# How long in seconds should the robot pause at each location?
self.rest_time = rospy.get_param("~rest_time", 3)
# Are we running in the fake simulator?
self.fake_test = rospy.get_param("~fake_test", False)
# Goal state return values
goal_states = [
'PENDING', 'ACTIVE', 'PREEMPTED', 'SUCCEEDED', 'ABORTED',
'REJECTED', 'PREEMPTING', 'RECALLING', 'RECALLED', 'LOST'
]
# Set up the goal locations. Poses are defined in the map frame.
# An easy way to find the pose coordinates is to point-and-click
# Nav Goals in RViz when running in the simulator.
#
# Pose coordinates are then displayed in the terminal
# that was used to launch RViz.
locations = dict()
#locations['one-1'] = Pose(Point(1.1952,-0.1422,0.0), Quaternion(0.0,0.0,0.1744,0.9847))
#locations['one'] = Pose(Point(1.86,-1.247,0.0), Quaternion(0.0,0.0,0.8481,0.5298))
# locations['two-1'] = Pose(Point(1.3851,-0.1253,0.0), Quaternion(0.0,0.0,0.9818,-0.1901))
# locations['two-2'] = Pose(Point(-0.4032,-0.9106,0.0), Quaternion(0.0,0.0,-0.5523,0.8337))
# locations['two'] = Pose(Point(-0.0019,-1.9595,0.0), Quaternion(0.0,0.0,-0.5236,0.8520))
# locations['three-1'] = Pose(Point(-0.4032,-0.9106,0.0), Quaternion(0.0,0.0,-0.5523,0.8337))
# locations['three-2'] = Pose(Point(0.2382,0.0559,0.0), Quaternion(0.0,0.0,0.2464,0.9692))
locations['three'] = Pose(Point(1.0602, 0.5279, 0.0),
Quaternion(0.0, 0.0, 0.2534, 0.9674))
# locations['four'] = Pose(Point(1.0602,0.5279,0.0), Quaternion(0.0,0.0,0.2534,0.9674))
locations['three-2'] = Pose(Point(0.2382, 0.0559, 0.0),
Quaternion(0.0, 0.0, 0.2464,
0.9692)) #initialpose
locations['initial'] = locations['three-2']
# Publisher to manually control the robot (e.g. to stop it)
self.cmd_vel_pub = rospy.Publisher('cmd_vel', Twist, queue_size=10)
self.IOTnet_pub = rospy.Publisher('/IOT_cmd', IOTnet, queue_size=10)
self.initial_pub = rospy.Publisher('initialpose',
PoseWithCovarianceStamped,
queue_size=10)
# Subscribe to the move_base action server
self.move_base = actionlib.SimpleActionClient("move_base",
MoveBaseAction)
rospy.loginfo("Waiting for move_base action server...")
# Wait 60 seconds for the action server to become available
self.move_base.wait_for_server(rospy.Duration(60))
rospy.loginfo("Connected to move base server")
# A variable to hold the initial pose of the robot to be set by
# the user in RViz
initial_pose = PoseWithCovarianceStamped()
# Variables to keep track of success rate, running time,
# and distance traveled
n_locations = len(locations)
n_goals = 0
n_successes = 0
i = 0
distance_traveled = 0
start_time = rospy.Time.now()
running_time = 0
location = ""
last_location = ""
sequeue = ['three']
# Get the initial pose from the user
#rospy.loginfo("*** Click the 2D Pose Estimate button in RViz to set the robot's initial pose...")
#rospy.wait_for_message('initialpose', PoseWithCovarianceStamped)
self.last_location = Pose()
rospy.Subscriber('initialpose', PoseWithCovarianceStamped,
self.update_initial_pose)
setpose = PoseWithCovarianceStamped(
Header(0, rospy.Time(), "map"),
PoseWithCovariance(locations['initial'], [
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
]))
self.initial_pub.publish(setpose)
# Make sure we have the initial pose
rospy.sleep(1)
while initial_pose.header.stamp == "":
rospy.sleep(1)
rospy.sleep(1)
# locations['back'] = Pose()
rospy.loginfo("Starting position navigation ")
# Begin the main loop and run through a sequence of locations
while not rospy.is_shutdown():
# If we've gone through the all sequence, then exit
if i == n_locations:
rospy.logwarn("Now reach all destination, now exit...")
rospy.signal_shutdown('Quit')
break
# Get the next location in the current sequence
location = sequeue[i]
# Keep track of the distance traveled.
# Use updated initial pose if available.
if initial_pose.header.stamp == "":
distance = sqrt(
pow(
locations[location].position.x -
locations[last_location].position.x, 2) + pow(
locations[location].position.y -
locations[last_location].position.y, 2))
else:
rospy.loginfo("Updating current pose.")
distance = sqrt(
pow(
locations[location].position.x -
initial_pose.pose.pose.position.x, 2) + pow(
locations[location].position.y -
initial_pose.pose.pose.position.y, 2))
initial_pose.header.stamp = ""
# Store the last location for distance calculations
last_location = location
# Increment the counters
i += 1
n_goals += 1
# Set up the next goal location
self.goal = MoveBaseGoal()
self.goal.target_pose.pose = locations[location]
self.goal.target_pose.header.frame_id = 'map'
self.goal.target_pose.header.stamp = rospy.Time.now()
# Let the user know where the robot is going next
rospy.loginfo("Going to: " + str(location))
# Start the robot toward the next location
self.move_base.send_goal(self.goal) #move_base.send_goal()
# Allow 5 minutes to get there
finished_within_time = self.move_base.wait_for_result(
rospy.Duration(300))
# Map to 4 point nav
cur_position = -1
position_seq = ['one']
if str(location) in position_seq:
cur_position = position_seq.index(str(location)) + 1
# Check for success or failure
if not finished_within_time:
self.move_base.cancel_goal() #move_base.cancle_goal()
rospy.loginfo("Timed out achieving goal")
else:
state = self.move_base.get_state() #move_base.get_state()
if state == GoalStatus.SUCCEEDED:
rospy.loginfo("Goal succeeded!")
n_successes += 1
distance_traveled += distance
rospy.loginfo("State:" + str(state))
# if cur_position!=-1:
# self.IOTnet_pub.publish(cur_position)
# rospy.sleep(12)
# if cur_position == 2:
# rospy.sleep(5)
else:
rospy.loginfo("Goal failed with error code: " +
str(goal_states[state]))
# if cur_position != -1:
# self.IOTnet_pub.publish(cur_position)
# rospy.sleep(12)
# if cur_position == 2:
# rospy.sleep(5)
# How long have we been running?
running_time = rospy.Time.now() - start_time
running_time = running_time.secs / 60.0
# Print a summary success/failure, distance traveled and time elapsed
rospy.loginfo("Success so far: " + str(n_successes) + "/" +
str(n_goals) + " = " +
str(100 * n_successes / n_goals) + "%")
rospy.loginfo("Running time: " + str(trunc(running_time, 1)) +
" min Distance: " +
str(trunc(distance_traveled, 1)) + " m")
rospy.sleep(self.rest_time)
class Car:
global console_serG
poseFromAmcl = PoseWithCovarianceStamped()
# manually defined waypoints
waypoint_x = [
9.22297477722, 9.5609331131, 9.82821846008, 10.0760278702,
10.3985872269, 10.8705215454, 11.1512451172, 11.4630308151,
11.7872953415, 12.0715732574, 12.5092477798, 12.9083614349,
12.959431648, 13.0315103531, 13.0642805099, 13.0588378906,
13.0395879745
]
waypoint_y = [
0.0268885232508, 0.00712374784052, -0.00839765090495, -0.0787715613842,
-0.046705879271, -0.085138015449, -0.136199742556, -0.169776380062,
-0.0992393344641, -0.135954737663, -0.112006239593, 0.361669361591,
0.636179447174, 0.964450240135, 1.27673280239, 1.62345945835,
1.95718026161
]
my_x = 0
my_y = 0
waypoint_index = 0
##
run_itr = 0
## PID_steer oy y parameters :
dist_goal = 0.0
err_old1 = 0
err1 = 0
err_sum1 = 0
d_g = 0.0
Kp1 = 25
Kd1 = 1
Ki1 = 0
y_low = -10
y_high = 10
## PID_fwd or x parameters
dist_goal = 0.0
err_old2 = 0
err2 = 0
err_sum2 = 0
d_g = 0.0
Kp2 = 10
Kd2 = 0
Ki2 = 0
x_low = -20
x_high = 20
## car hardware initializations
mode = 'A'
## imu initializations
del_t = 0.0067 # for 150 Hz
val_dx = 0
val_dy = 0
val_dz = 0
val_vx = 0
val_vy = 0
val_vz = 0
val_ax = 0
val_ay = 0
val_az = 0
bias_ax = 0
## control the heading; y(or steer) then x(or fwd)
def run(self):
val_x = 0
val_y = 0
waypoint_number = 9
# choose val_x and val_y as per waypoint index
val_x = self.waypoint_x[self.waypoint_index]
val_y = self.waypoint_y[self.waypoint_index]
# log iterations
self.run_itr = self.run_itr + 1
flag = True
print('before IMU')
# use imu to uypdate my_x and my_y
console_serG.write('IMU1')
while not rospy.is_shutdown() and flag == True:
char = console_serG.read()
print('reading char')
if char == 'I':
print('inside if')
self.val_ax = console_serG.read(6)
self.val_ax = float(self.val_ax)
self.val_ax = self.val_ax / 32768 * 2 * 9.8
self.val_ax_cor = self.val_ax - self.bias_ax
self.val_vx = self.val_vx + self.val_ax_cor * self.del_t
self.val_dx = self.val_dx + self.val_vx * self.del_t + self.val_ax * self.del_t * self.del_t * 0.5
print(' ax_cor:{} \t ax:{} \t dx:{}'.format(
self.val_ax_cor, self.val_ax, self.val_dx))
flag = False
print('imu complete')
self.my_x = self.val_dx
self.err1 = val_y - self.my_y
y_cor = self.Kp1 * (self.err1) + self.Kd1 * (
self.err1 - self.err_old1) + self.Ki1 * (self.err_sum1)
# bracket the y_cor or steer
if y_cor < self.y_low:
y_cor = self.y_low
elif y_cor > self.y_high:
y_cor = self.y_high
# scale to -2048 to 2048
self.steer = y_cor / self.y_high * 2048
self.steer = int(self.steer)
# note the steer_direction for string formingw
if self.steer < 0 or self.steer == 0:
steer_direction = '+'
self.steer_abs = abs(self.steer)
elif self.steer > 0:
steer_direction = '-'
self.steer_abs = abs(self.steer)
self.steer = self.int_to_str_steer(self.steer_abs)
self.err_sum1 = self.err1 + self.err_sum1
self.err_old1 = self.err1
self.err2 = val_x - self.my_x
x_cor = self.Kp2 * (self.err2) + self.Kd2 * (
self.err2 - self.err_old2) + self.Ki2 * (self.err_sum2)
# bracket the x_cor or fwd
if x_cor < self.x_low:
x_cor = self.x_low
elif x_cor > self.x_high:
x_cor = self.x_high
# scale to -2048 to 2048
self.fwd = x_cor / self.x_high * 2048
self.fwd = int(self.fwd)
# note the direction for string forming
if self.fwd < 0 or self.fwd == 0:
fwd_direction = '-'
self.fwd_abs = abs(self.fwd) # fwd values cannot be negative
self.fwd_abs = 0
elif self.steer > 0:
fwd_direction = '+'
self.fwd_abs = abs(self.fwd)
if self.fwd_abs < 300 and self.fwd != 0: # drive motor dead
self.fwd_abs = 300
self.fwd = self.int_to_str_fwd(self.fwd_abs)
self.err_sum2 = self.err2 + self.err_sum2
self.err_old2 = self.err2
cmd = self.mode + steer_direction + self.steer + fwd_direction + self.fwd
print("cmd:{}='A+s+f'| /pose x = {}, y = {} || wp_no: {}, itr: {}".
format(cmd, self.my_x, self.my_y, self.waypoint_index,
self.run_itr))
console_serG.write(cmd)
print('command writing complete')
## This function takkes an integer and converts it into a string. It also adds leading zeros to make sure the string is in 4 digit format. The number entered in this function must be >=0.
def int_to_str_steer(self, steer_abs):
my_int = self.steer_abs
if my_int > 999:
my_int = str(my_int)
elif (my_int < 999 or my_int == 999) and (my_int > 100
or my_int == 100):
my_int = '0' + str(my_int)
elif my_int < 100 and my_int > 9:
my_int = '00' + str(my_int)
elif my_int < 9 and my_int > 0:
my_int = '000' + str(my_int)
elif my_int == 0:
my_int = '0000'
my_int = str(my_int)
return my_int
def int_to_str_fwd(self, fwd_abs):
my_int = self.fwd_abs
if my_int > 999:
my_int = str(my_int)
elif (my_int < 999 or my_int == 999) and (my_int > 100
or my_int == 100):
my_int = '0' + str(my_int)
elif my_int < 100 and my_int > 9:
my_int = '00' + str(my_int)
elif my_int < 9 and my_int > 0:
my_int = '000' + str(my_int)
elif my_int == 0:
my_int = '0000'
my_int = str(my_int)
return my_int
class GetGoal:
#Initial pose
pose_stamped=PoseWithCovarianceStamped()
pose_stamped.pose.pose.position.x=5.0
pose_stamped.pose.pose.position.y=5.0
pose_stamped.pose.pose.orientation.z=0
map=OccupancyGrid()
map_array=[[i for i in range(4000)],[j for j in range(4000)]]
#map_array=[[],[]]
Goal_is_Obtained = False
def __init__(self):
# Creates a node with
rospy.init_node('GetGoal', anonymous=True)
# Publisher which will publish to the topic 'Goal'.
self.goal_publisher = rospy.Publisher('/move_base/goal', MoveBaseActionGoal, queue_size=10)
# A subscriber to the topics 'cmd_vel'.
#self.velocity_subscriber=rospy.Subscriber("/cmd_vel",Twist, self.timer_callback) # When receiving a message, call timer_callback()
self.velocity_subscriber=rospy.Subscriber("/cmd_vel",Twist)
# A subscriber to the topics '/amcl_Pose'
self.pose_subscriber = rospy.Subscriber('/amcl_pose', PoseWithCovarianceStamped, self.update_pose)
# A subscriber to the topics '/map'
self.pose_subscriber = rospy.Subscriber('/map', OccupancyGrid, self.update_map)
def timer_callback(self, msg):
global timer
print("Velocity message received")
timer.cancel()
timer = threading.Timer(2,self.PublishGoal) #2 seconds elapse
timer.start()
return
def update_pose(self, msg):
print("Map message recieved")
self.pose_stamped=msg
#self.pose_stamped.pose.pose.position.x
#self.pose_stamped.pose.pose.position.y
#self.pose_stamped.pose.pose.orientation.z
def update_map(self, msg):
print("Map message recieved")
self.map=msg
if not self.map.data: print("Map data is empty"); return
if self.map.info.width == 640: return
w=self.map.info.width
h=self.map.info.heights
for i in range(h):
for j in range(w):
try:
self.map_array[i][j]=self.map.data[i*w+j-1]
except IndexError:
#print (i,j,self.map_array[i][j],self.map.data[i*w+j-1]) adress to self.map_array cause IndexError(out of range)
self.PublishGoal
def PublishGoal(self):
x=self.pose_stamped.pose.pose.position.x
y=self.pose_stamped.pose.pose.position.y
theta=self.pose_stamped.pose.pose.orientation.z*PI
#res=self.map.info.resolution
res=0.05
i=3
sum=0
self.Goal_is_Obtained = True
xc=int(x/res)-1 #x-check coordinate
yc=int(y/res)-1
#map analysys
while self.Goal_is_Obtained: #checks 8 sqrs around current position
try:
if self.check_sqr(xc+i, yc) < sum: sum=self.check_sqr(xc+i , yc); xg=xc+i; yg=yc;
if self.check_sqr(xc-i, yc) < sum: sum=self.check_sqr(xc-i , yc); xg=xc-i; yg=yc;
if self.check_sqr(xc, yc+i) < sum: sum=self.check_sqr(xc , yc+i); xg=xc; yg=yc+i;
if self.check_sqr(xc, yc-i) < sum: sum=self.check_sqr(xc+i , yc-i); xg=xc; yg=yc-i;
if self.check_sqr(xc+i, yc+i) < sum: sum=self.check_sqr(xc+i , yc+i); xg=xc+i; yg=yc+i;
if self.check_sqr(xc+i, yc-i) < sum: sum=self.check_sqr(xc+i , yc-i); xg=xc+i; yg=yc-i;
if self.check_sqr(xc-i , yc+i) < sum: sum=self.check_sqr(xc-i , yc+i); xg=xc-i; yg=yc+i;
if self.check_sqr(xc-i , yc-i) < sum: sum=self.check_sqr(xc-i , yc-i); xg=xc-i; yg=yc-i;
if sum < 0: self.Goal_is_Obtained = False
else: i=i+1
except IndexError:
pass
#publishing goal
print("Publishing goal")
pose_msg=MoveBaseActionGoal
pose_msg.goal.pose.position.x = xg*res
pose_msg.goal.pose.position.y = yg*res
#print(xg*res, yg*res)
self.goal_publisher.pubish(pose_msg)
return
def check_sqr(self,x,y): #asssumes sqr 3x3 is known or not summurazing coast map
#self.map_array=self.map.data
sum=self.map_array[x][y]+self.map_array[x+1][y]+self.map_array[x][y+1]+self.map_array[x-1][y]+self.map_array[x][y-1]+self.map_array[x+1][y+1]+self.map_array[x+1][y-1]+self.map_array[x-1][y+1]+self.map_array[x-1][y-1]
return sum
def get_lsq_triangulation_error(self,observation,epsilon=0.5):
# Get the camera intrinsics of both cameras
P1 = self.get_cam_intrinsic()
P2 = self.get_neighbor_cam_intrinsic()
# Convert world coordinates to local camera coordinates for both cameras
# We get the camera extrinsics as follows
trans,rot = self.listener.lookupTransform(self.rotors_machine_name+'/xtion_rgb_optical_frame','world', rospy.Time(0)) #target to source frame
(r,p,y) = tf.transformations.euler_from_quaternion(rot)
H1 = tf.transformations.euler_matrix(r,p,y,axes='sxyz')
H1[0:3,3] = trans
print(str(self.env_id)+' '+str(self.rotors_machine_name)+':'+str(trans))
trans,rot = self.listener.lookupTransform(self.rotors_neighbor_name+'/xtion_rgb_optical_frame','world', rospy.Time(0)) #target to source frame
(r,p,y) = tf.transformations.euler_from_quaternion(rot)
H2 = tf.transformations.euler_matrix(r,p,y,axes='sxyz')
H2[0:3,3] = trans
print(str(self.env_id)+' '+str(self.rotors_neighbor_name)+':'+str(trans))
#Concatenate Intrinsic Matrices
intrinsic = np.array((P1[0:3,0:3],P2[0:3,0:3]))
#Concatenate Extrinsic Matrices
extrinsic = np.array((H1,H2))
joints_gt = self.get_joints_gt()
error = 0
self.triangulated_root = PoseArray()
for k,v in dict_joints.items():
p2d1 = self.joints[dict_joints[k],0:2]
p2d2 = self.joints_neighbor[dict_joints[k],0:2]
points_2d = np.array((p2d1,p2d2))
# Solve system of equations using least squares to estimate person position in robot 1 camera frame
estimate_root = self.lstsq_triangulation(intrinsic,extrinsic,points_2d,2)
es_root_cam = Point();es_root_cam.x = estimate_root[0];es_root_cam.y = estimate_root[1];es_root_cam.z = estimate_root[2]
err_cov = PoseWithCovarianceStamped()
joints = 0
if v>4: #because head, eyes and ears lead to high error for the actor
# if v==5 or v == 6:
p = Pose()
p.position = es_root_cam
self.triangulated_root.poses.append(p)
gt = joints_gt.poses[alpha_to_gt_joints[k]].position
error += (self.get_distance_from_point(gt,es_root_cam))
err_cov.pose.pose.position = es_root_cam
err_cov.pose.covariance[0] = (gt.x - es_root_cam.x)**2
err_cov.pose.covariance[7] = (gt.y - es_root_cam.y)**2
err_cov.pose.covariance[14] = (gt.z - es_root_cam.z)**2
err_cov.header.stamp = rospy.Time()
err_cov.header.frame_id = 'world'
self.triangulated_cov_pub[alpha_to_gt_joints[k]].publish(err_cov)
joints+=1
# Publish all estimates
self.triangulated_root.header.stamp = rospy.Time()
self.triangulated_root.header.frame_id = 'world'
self.triangulated_pub.publish(self.triangulated_root)
is_in_desired_pos = False
error = error/joints
if error<=epsilon:
# error = 0.4*error/epsilon
is_in_desired_pos = True
# else:
# error = 0.4
return [is_in_desired_pos,error]
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 )
def __init__(self):
# soundhandle = SoundClient()
rospy.init_node('nav_test', anonymous=True)
rospy.on_shutdown(self.shutdown)
# How long in seconds should the robot pause at each location?
self.rest_time = rospy.get_param("~rest_time", 1.5)
# Are we running in the fake simulator?
self.fake_test = rospy.get_param("~fake_test", False)
# Goal state return values
goal_states = ['PENDING', 'ACTIVE', 'PREEMPTED',
'SUCCEEDED', 'ABORTED', 'REJECTED',
'PREEMPTING', 'RECALLING', 'RECALLED',
'LOST']
# Set up the goal locations. Poses are defined in the map frame.
# An easy way to find the pose coordinates is to point-and-click
# Nav Goals in RViz when running in the simulator.
# Pose coordinates are then displayed in the terminal
# that was used to launch RViz.
locations = OrderedDict();
# ---------------4 POINTS-----------------------------------------------------------
# # Locations for bigger simulation map
locations['PIT STOP 1'] = Pose(Point(1.6, -2.5, 0.000), Quaternion(0.000, 0.000, 0.733, 0.680))
locations['PIT STOP 2'] = Pose(Point(1.7, 2.5, 0.000), Quaternion(0.000, 0.000, 0.223, 0.975))
locations['PIT STOP 3'] = Pose(Point(-3.3, 2.5, 0.000), Quaternion(0.000, 0.000, -0.670, 0.743))
locations['PIT STOP 4'] = Pose(Point(-3.5, -2.6, 0.000), Quaternion(0.000, 0.000, 0.786, 0.618))
# # Locations for real lab environment (low precision) real1
# locations['PIT STOP 1'] = Pose(Point(0.5, 1, 0.000), Quaternion(0.000, 0.000, 0.733, 0.680))
# locations['PIT STOP 1.5'] = Pose(Point(1.7, 1.8, 0.000), Quaternion(0.000, 0.000, -0.02, 1))
# locations['PIT STOP 2'] = Pose(Point(4.43, 1.08, 0.000), Quaternion(0.000, 0.000, 0.75, 0))
# # locations['SCENIC SPOT 0.5'] = Pose(Point(3.4, 1.08, 0.000), Quaternion(0.000, 0.000, 0.75, 0))
# locations['SCENIC SPOT 1'] = Pose(Point(2.9, 1.08, 0.000), Quaternion(0.000, 0.000, 0.75, 0))
# locations['PIT STOP 3'] = Pose(Point(4.43, 5.15, 0.000), Quaternion(0.000, 0.000, 0.75, 0.6))
# locations['SCENIC SPOT 2'] = Pose(Point(4.69, 4.14, 0.000), Quaternion(0.000, 0.000, 0.99, 0))
# locations['PIT STOP 4'] = Pose(Point(0.47, 5.05, 0.000), Quaternion(0.000, 0.000, 0.75, -3))
# # Locations for real lab environment (HI precision) real4
# # # locations['PIT STOP 1'] = Pose(Point(0.7, -0.85, 0.000), Quaternion(0.000, 0.000, 0, 1))
# # locations['PIT STOP 1.5'] = Pose(Point(0.97, 0.69, 0.000), Quaternion(0.000, 0.000, -0.015, 0.9998))
# locations['PIT STOP 2'] = Pose(Point(4.08, -0.2, 0.000), Quaternion(0.000, 0.000, 0.9998, 0.016))
# # locations['SCENIC SPOT 0.5'] = Pose(Point(2.730, -0.102, 0.000), Quaternion(0.000, 0.000, 0.999, 0.008))
# locations['SCENIC SPOT 1'] = Pose(Point(2.847, -0.081, 0.000), Quaternion(0.000, 0.000, 0.9999, 0.027))
# locations['PIT STOP 3'] = Pose(Point(4.33, 3.85, 0.000), Quaternion(0.000, 0.000, 0.898, -0.4386))
# locations['SCENIC SPOT 2'] = Pose(Point(4.294, 3.0922, 0.000), Quaternion(0.000, 0.000, 0.9987, -0.049))
# # locations['PIT STOP 4'] = Pose(Point(0.2, 4, 0.000), Quaternion(0.000, 0.000, -0.031, 0.999))
# --------------------------------------------------------------------------
# Publisher to manually control the robot (e.g. to stop it)
self.cmd_vel_pub = rospy.Publisher('cmd_vel', Twist)
# Subscribe to the move_base action server
self.move_base = actionlib.SimpleActionClient("move_base", MoveBaseAction)
rospy.loginfo("Waiting for move_base action server...")
# Wait 60 seconds for the action server to become available
self.move_base.wait_for_server(rospy.Duration(60))
rospy.loginfo("Connected to move base server")
# A variable to hold the initial pose of the robot to be set by
# the user in RViz
initial_pose = PoseWithCovarianceStamped()
# Variables to keep track of success rate, running time,
# and distance traveled
n_locations = len(locations)
n_goals = 0
n_successes = 0
i = n_locations
distance_traveled = 0
start_time = rospy.Time.now()
running_time = 0
location = ""
last_location = ""
# Get the initial pose from the user
rospy.loginfo("*** Click the 2D Pose Estimate button in RViz to set the robot's initial pose...")
rospy.wait_for_message('initialpose', PoseWithCovarianceStamped)
self.last_location = Pose()
rospy.Subscriber('initialpose', PoseWithCovarianceStamped, self.update_initial_pose)
# Make sure we have the initial pose
while initial_pose.header.stamp == "":
rospy.sleep(1)
rospy.loginfo("Starting navigation test")
# Begin the main loop and run through a sequence of locations
while not rospy.is_shutdown():
# If we've gone through the current sequence,
# start with a new random sequence
if i == n_locations:
i = 0
# sequence = sample(locations, n_locations) # RANDOMIZE(only keys no value)
sequence = locations.keys()
# Skip over first location if it is the same as
# the last location
if sequence[0] == last_location:
i = 1
#--------------------------------
rospy.loginfo("ORDER: %s", str(sequence))
# Get the next location in the current sequence
location = sequence[i]
# Keep track of the distance traveled.
# Use updated initial pose if available.
if initial_pose.header.stamp == "":
distance = sqrt(pow(locations[location].position.x -
locations[last_location].position.x, 2) +
pow(locations[location].position.y -
locations[last_location].position.y, 2))
else:
rospy.loginfo("Updating current pose.")
distance = sqrt(pow(locations[location].position.x -
initial_pose.pose.pose.position.x, 2) +
pow(locations[location].position.y -
initial_pose.pose.pose.position.y, 2))
initial_pose.header.stamp = ""
# Store the last location for distance calculations
last_location = location
# Increment the counters
i += 1
n_goals += 1
# Set up the next goal location
self.goal = MoveBaseGoal()
self.goal.target_pose.pose = locations[location]
self.goal.target_pose.header.frame_id = 'map'
self.goal.target_pose.header.stamp = rospy.Time.now()
# Let the user know where the robot is going next
rospy.loginfo("Going to: " + str(location))
# Start the robot toward the next location
self.move_base.send_goal(self.goal)
# Allow 5 minutes to get there
finished_within_time = self.move_base.wait_for_result(rospy.Duration(300))
# Check for success or failure
if not finished_within_time:
self.move_base.cancel_goal()
rospy.loginfo("Timed out achieving goal")
else:
state = self.move_base.get_state()
if state == GoalStatus.SUCCEEDED:
rospy.loginfo("Goal succeeded!")
n_successes += 1
distance_traveled += distance
rospy.loginfo("State:" + str(state))
else:
rospy.loginfo("Goal failed with error code: " + str(goal_states[state]))
# How long have we been running?
# running_time = rospy.Time.now() - start_time
# running_time = running_time.secs / 60.0
# Print a summary success/failure, distance traveled and time elapsed
rospy.loginfo("Success so far: " + str(n_successes) + "/" +
str(n_goals) + " = " +
str(100 * n_successes/n_goals) + "%")
# rospy.loginfo("Running time: " + str(trunc(running_time, 1)) +
# " min Distance: " + str(trunc(distance_traveled, 1)) + " m")
# soundhandle.playWave('stop.m4a',self.rest_time)
rospy.sleep(self.rest_time)
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 cb_ground_truth(self, msg):
self.ground_truth = PoseWithCovarianceStamped()
#print str(self.ground_truth)
self.ground_truth.header.frame_id = "/map"
self.ground_truth.pose.pose = msg.pose.pose
def __init__(self):
rospy.init_node('nav_test', anonymous=True)
rospy.on_shutdown(self.shutdown)
# How long in seconds should the robot pause at each location?
self.rest_time = rospy.get_param("~rest_time", 2)
# Are we running in the fake simulator?
self.fake_test = rospy.get_param("~fake_test", False)
# Goal state return values
goal_states = [
'PENDING', 'ACTIVE', 'PREEMPTED', 'SUCCEEDED', 'ABORTED',
'REJECTED', 'PREEMPTING', 'RECALLING', 'RECALLED', 'LOST'
]
locations = dict()
#定义四个地点的位置
locations['起始点'] = Pose(Point(1.309, 0.919, 0.000),
Quaternion(0.000, 0.000, 0.000, 1.000))
locations['客厅'] = Pose(Point(3.199, 0.513, 0.000),
Quaternion(00.000, 0.000, 0.000, 1.000))
locations['厨房'] = Pose(Point(5.581, 0.977, 0.000),
Quaternion(0.000, 0.000, 0.752, 0.660))
locations['卧室'] = Pose(Point(6.223, -2.749, 0.000),
Quaternion(0.000, 0.000, 0.985, -0.170))
self.cmd_vel_pub = rospy.Publisher('cmd_vel', Twist, queue_size=5)
# Subscribe to the move_base action server
self.move_base = actionlib.SimpleActionClient("move_base",
MoveBaseAction)
rospy.loginfo("Waiting for move_base action server...")
# Wait 60 seconds for the action server to become available
self.move_base.wait_for_server(rospy.Duration(60))
rospy.loginfo("Connected to move base server")
# A variable to hold the initial pose of the robot to be set by
# the user in RViz
initial_pose = PoseWithCovarianceStamped()
# Variables to keep track of success rate, running time,
# and distance traveled
n_loop = 0 #定义控制循环，当为1时停止循环
first = 0 #定义第一次变量
n_locations = len(locations)
n_goals = 0
n_successes = 0
i = n_locations
distance_traveled = 0
start_time = rospy.Time.now()
running_time = 0
location = ""
last_location = ""
self.Dist = 0
self.Asr = ""
Loc_1 = 0
Loc_2 = 0
Loc_3 = 0
Name_1 = 0
name_2 = 0
Name = ""
Drink_1 = 0
Drink_2 = 0
Drink_3 = 0
Drink = ""
# Get the initial pose from the user (how to set)
rospy.loginfo(
"*** Click the 2D Pose Estimate button in RViz to set the robot's initial pose..."
)
rospy.wait_for_message('initialpose', PoseWithCovarianceStamped)
self.last_location = Pose()
rospy.Subscriber('initialpose', PoseWithCovarianceStamped,
self.update_initial_pose)
# Make sure we have the initial pose
while initial_pose.header.stamp == "":
rospy.sleep(1)
rospy.loginfo("Starting navigation test")
#dict_keys = ['起始点','厨房','卧室']
dict_keys = ['客厅', '卧室', '厨房']
pub1 = rospy.Publisher('/voice/xf_tts_topic', String,
queue_size=5) #tts
pub2 = rospy.Publisher('/voice/xf_asr_topic', Int32,
queue_size=5) #asr
#pub3 = rospy.Publisher('arm',String,queue_size=5) #arm
#pub4 = rospy.Publisher('tracking',Int32,queue_size=5) #物体识别
pub5 = rospy.Publisher('/following', Int32, queue_size=5) #人体跟随
#pub6 = rospy.Publisher('drink_type',String,queue_size=5) #拿的物体类别
pub7 = rospy.Publisher('ros2_wake', Int32, queue_size=5) #ros2opencv2
#pub8 = rospy.Publisher('/voice/xf_nav_follow',String,queue_size=5) #nav_follow
#######################################
# Begin the main loop and run through a sequence of locations
while n_loop != 1:
if first == 0: #如果是第一次
i = 0
location = "起始点"
first += 1
# sequence = sample(locations, n_locations)
# Skip over first location if it is the same as
# the last location
#if dict_keys[0] == last_location:
# i = 1
#####################################
####### Modify by XF 2017.4.14 ######
# location = sequence[i]
# rospy.loginfo("location= " + str(location))
# location = locations.keys()[i]
#location = dict_keys[i]
#####################################
# Keep track of the distance traveled.
# Use updated initial pose if available.
if initial_pose.header.stamp == "":
distance = sqrt(
pow(
locations[location].position.x -
locations[last_location].position.x, 2) + pow(
locations[location].position.y -
locations[last_location].position.y, 2))
else:
rospy.loginfo("Updating current pose.")
distance = sqrt(
pow(
locations[location].position.x -
initial_pose.pose.pose.position.x, 2) + pow(
locations[location].position.y -
initial_pose.pose.pose.position.y, 2))
initial_pose.header.stamp = ""
# Store the last location for distance calculations
#last_location = location
#rospy.loginfo("test 1 last_location="+str(last_location))
# Increment the counters
#i += 1
#rospy.loginfo("test 2 i="+str(i))
n_goals += 1
# Set up the next goal location
self.goal = MoveBaseGoal()
self.goal.target_pose.pose = locations[location]
self.goal.target_pose.header.frame_id = 'map'
self.goal.target_pose.header.stamp = rospy.Time.now()
# Let the user know where the robot is going next
rospy.loginfo("Going to: " + str(location))
# Start the robot toward the next location
self.move_base.send_goal(self.goal)
# Allow 6 minutes to get there
finished_within_time = self.move_base.wait_for_result(
rospy.Duration(360))
# Check for success or failure
if not finished_within_time:
self.move_base.cancel_goal()
rospy.loginfo("Timed out achieving goal")
else:
state = self.move_base.get_state()
if state == GoalStatus.ABORTED: # can not find a plan,give feedback
rospy.loginfo("please get out")
status_n = "please get out"
pub1.publish(status_n)
if state == GoalStatus.SUCCEEDED:
rospy.loginfo("Goal succeeded!")
n_successes += 1
distance_traveled += distance
rospy.loginfo("State:" + str(state))
############ add voice by XF 4.25 ################## The 1st
# pub1 = rospy.Publisher('/voice/xf_tts_topic', String, queue_size=5)
loc = "主人，我已到" + str(location)
pub1.publish(loc) #tts
rospy.sleep(5)
# rospy.Subscriber('dist',Int32,self.callbackDist) #now robot already adjust correctly
# rospy.loginfo("Dist:"+str(self.Dist))
commandA1 = "您好，请问有什么指示?"
pub1.publish(commandA1)
rospy.sleep(3)
#while self.Asr == "":
#awake=1
#pub2.publish(awake) #asr
#rospy.Subscriber('/voice/xf_asr_follow',String,self.callbackAsr) #获取asr发过来的说话内容
#rospy.sleep(10)
#rospy.loginfo("self.Asr:"+str(self.Asr))
#if self.Asr != "":
# self.Asr=""
#while self.Asr == "":
awake = 1
pub2.publish(awake) #asr
rospy.Subscriber('/voice/xf_asr_follow', String,
self.callbackAsr) #获取asr发过来的说话内容
rospy.sleep(10)
rospy.loginfo("self.Asr:" + str(self.Asr))
#if self.Asr != "":
# self.Asr=""
#rospy.sleep(10)
Loc_1 = string.find(
self.Asr, '客厅') #find函数，返回所查字符串在整个字符串的起始位置，如果没有，则返回-1
Loc_2 = string.find(self.Asr, '厨房')
Loc_3 = string.find(self.Asr, '卧室')
#Name_1=string.find(self.Asr,'郭晓萍')
#Name_2=string.find(self.Asr,'方璐')
# Drink_1=string.find(self.Asr,'可乐')
# Drink_2=string.find(self.Asr,'雪碧')
# Drink_3=string.find(self.Asr,'橙汁')
if Loc_1 != -1:
rospy.loginfo("匹配客厅")
#dict_keys = [last_location,'客厅']
last_location = location
location = "客厅"
if Loc_2 != -1:
rospy.loginfo("匹配厨房")
last_location = location
location = "厨房"
#dict_keys = [last_location,'卧室']
if Loc_3 != -1:
rospy.loginfo("匹配卧室")
last_location = location
#dict_keys=[last_location,'厨房']
location = "卧室"
#if Name_1!=-1:
# Name = '郭晓萍'
# rospy.loginfo("匹配郭晓萍")
#if Name_2!=-1:
# Name = '方璐'
# rospy.loginfo("匹配方璐")
# if Drink_1!=-1:
# Drink = '可乐'
# if Drink_2!=-1:
# Drink = '雪碧'
# if Drink_3!=-1:
# Drink = '橙汁'
# ros2_wake=1
# pub7.publish(ros2_wake)
# pub6.publish(Drink)
#######################################
self.Asr = ""
rospy.sleep(5)
commandA2 = "收到指示，请稍等."
pub1.publish(commandA2)
else:
rospy.loginfo("Goal failed with error code: " +
str(goal_states[state]))
# How long have we been running?
running_time = rospy.Time.now() - start_time
running_time = running_time.secs / 60.0
# Print a summary success/failure, distance traveled and time elapsed
rospy.loginfo("Success so far: " + str(n_successes) + "/" +
str(n_goals) + " = " + str(100 * n_successes / n_goals) +
"%")
rospy.loginfo("Running time: " + str(trunc(running_time, 1)) +
" min Distance: " + str(trunc(distance_traveled, 1)) +
" m")
rospy.sleep(self.rest_time)
init_pos.pose.pose.orientation.x, init_pos.pose.pose.orientation.y,
init_pos.pose.pose.orientation.z, -init_pos.pose.pose.orientation.w
]
q2 = quaternion_from_euler(0, 0, 0)
qr = tf.transformations.quaternion_multiply(q2, q1_inv)
t.transform.rotation.x = qr[0]
t.transform.rotation.y = qr[1]
t.transform.rotation.z = qr[2]
t.transform.rotation.w = qr[3]
br.sendTransform(t)
def init_map(init_hedge, init_imu):
init_pos.pose.pose.position = init_hedge.pose.pose.position
init_pos.pose.pose.orientation = init_imu.orientation
if __name__ == "__main__":
rospy.init_node("tf2_drift_publisher")
init_pos = PoseWithCovarianceStamped()
init_hedge = rospy.wait_for_message("/hedge_pose",
PoseWithCovarianceStamped)
init_imu = rospy.wait_for_message("/imu", Imu)
rospy.loginfo("Received Initial Hedge Pose")
init_map(init_hedge, init_imu)
rospy.spin()
def __init__(self):
rospy.on_shutdown(self.cleanup)
# Subscribe to the move_base action server
self.move_base = actionlib.SimpleActionClient("move_base", MoveBaseAction)
rospy.loginfo("Waiting for move_base action server...")
# Wait for the action server to become available
self.move_base.wait_for_server(rospy.Duration(120))
rospy.loginfo("Connected to move base server")
# A variable to hold the initial pose of the robot to be set by the user in RViz
initial_pose = PoseWithCovarianceStamped()
rospy.Subscriber('initialpose', PoseWithCovarianceStamped, self.update_initial_pose)
# Get the initial pose from the user
rospy.loginfo("*** Click the 2D Pose Estimate button in RViz to set the robot's initial pose...")
rospy.wait_for_message('initialpose', PoseWithCovarianceStamped)
# Make sure we have the initial pose
while initial_pose.header.stamp == "":
rospy.sleep(1)
rospy.loginfo("Ready to go")
rospy.sleep(1)
locations = dict()
# Location A
A_x = 1.0
A_y = 1.0
A_theta = 1.5708
quaternion = quaternion_from_euler(0.0, 0.0, A_theta)
locations['A'] = Pose(Point(A_x, A_y, 0.000), Quaternion(quaternion[0], quaternion[1], quaternion[2], quaternion[3]))
self.goal = MoveBaseGoal()
rospy.loginfo("Starting navigation test")
while not rospy.is_shutdown():
self.goal.target_pose.header.frame_id = 'map'
self.goal.target_pose.header.stamp = rospy.Time.now()
# Robot will go to point A
if start == 1:
rospy.loginfo("Going to point A")
rospy.sleep(2)
self.goal.target_pose.pose = locations['A']
self.move_base.send_goal(self.goal)
waiting = self.move_base.wait_for_result(rospy.Duration(300))
if waiting == 1:
rospy.loginfo("Reached point A")
rospy.sleep(2)
rospy.loginfo("Ready to go back")
rospy.sleep(2)
global start
start = 0
# After reached point A, robot will go back to initial position
elif start == 0:
rospy.loginfo("Going back home")
rospy.sleep(2)
self.goal.target_pose.pose = self.origin
self.move_base.send_goal(self.goal)
waiting = self.move_base.wait_for_result(rospy.Duration(300))
if waiting == 1:
rospy.loginfo("Reached home")
rospy.sleep(2)
global start
start = 2
rospy.Rate(5).sleep()
def initial_pos_pub(self):
# arucoCodeBroadcaster = TransformBroadcaster()
# arucoCodeBroadcaster.sendTransform(
# (6.56082, 6.63258, 0.0),
# (-0.0635783, -0.704243, -0.704243, -0.0635782),
# rospy.Time.now(),
# "aruco_code",
# "map"
# )
# this two line is used to stop the while loop
# when you using CTRL+C to exit the program
signal.signal(signal.SIGINT, quit)
signal.signal(signal.SIGTERM, quit)
# listener.waitForTransform("/map", "/camera_init", rospy.Time(), rospy.Duration(4.0))
# while not map_camera_trans:
# try:
# # rospy.sleep(1)
# # rospy.loginfo(map_camera_trans)
# time_now = rospy.Time.now()
# listener.waitForTransform("/map", "/camera_init", time_now, rospy.Duration(4.0))
# # listener.waitForTransform("/map", "/camera_init", rospy.Time(0), rospy.Duration(1.0))
# (map_camera_trans, map_camera_rot) = listener.lookupTransform('/map', '/camera_init', time_now)
# # (map_camera_trans, map_camera_rot) = listener.lookupTransform('/map', '/camera_init', rospy.Time(0))
# except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
# # map_camera_trans = [0, 0, 0]
# # map_camera_rot = [0, 0, 0, 0]
# # traceback.print_exc()
# rospy.loginfo("stop!!!-----------")
# continue
map_camera_trans = []
listener = tf.TransformListener()
rospy.sleep(0.1)
# count = 0
while not map_camera_trans:
try:
# rospy.loginfo(map_camera_trans)
# time_now = rospy.Time.now()
# listener.waitForTransform("/map", "/camera_init", time_now, rospy.Duration(5.0))
# listener.waitForTransform("/map", "/camera_init", rospy.Time(0), rospy.Duration(4.0))
# (map_camera_trans, map_camera_rot) = listener.lookupTransform('/map', '/camera_init', time_now)
(map_camera_trans, map_camera_rot) = listener.lookupTransform(
'/map', '/camera_init', rospy.Time(0))
rospy.loginfo("********Got Trans & Rot********")
except (tf.LookupException, tf.ConnectivityException,
tf.ExtrapolationException):
# except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException):
# map_camera_trans = [0, 0, 0]
# map_camera_rot = [0, 0, 0, 0]
# traceback.print_exc()
rospy.loginfo("********NO CODE --OR-- NO TF ********")
rospy.sleep(0.1)
# rospy.loginfo("stop!!!-----------")
continue
self.count = self.count + 1
self.trans_total[0] = self.trans_total[
0] + map_camera_trans[0] / self.NUM_USED_FOR_AVRG
self.trans_total[1] = self.trans_total[
1] + map_camera_trans[1] / self.NUM_USED_FOR_AVRG
self.rot_total[
0] = self.rot_total[0] + map_camera_rot[0] / self.NUM_USED_FOR_AVRG
self.rot_total[
1] = self.rot_total[1] + map_camera_rot[1] / self.NUM_USED_FOR_AVRG
self.rot_total[
2] = self.rot_total[2] + map_camera_rot[2] / self.NUM_USED_FOR_AVRG
self.rot_total[
3] = self.rot_total[3] + map_camera_rot[3] / self.NUM_USED_FOR_AVRG
# rospy.loginfo(map_camera_trans[0])
# rospy.loginfo(self.trans_total[0])
if self.count >= self.NUM_USED_FOR_AVRG:
rospy.loginfo(self.count)
self.trans_total_seq[0] = self.trans_total_seq[
0] + self.trans_total[0] / self.MAX_SEQ_NEED
self.trans_total_seq[1] = self.trans_total_seq[
1] + self.trans_total[1] / self.MAX_SEQ_NEED
self.rot_total_seq[0] = self.rot_total_seq[
0] + self.rot_total[0] / self.MAX_SEQ_NEED
self.rot_total_seq[1] = self.rot_total_seq[
1] + self.rot_total[1] / self.MAX_SEQ_NEED
self.rot_total_seq[2] = self.rot_total_seq[
2] + self.rot_total[2] / self.MAX_SEQ_NEED
self.rot_total_seq[3] = self.rot_total_seq[
3] + self.rot_total[3] / self.MAX_SEQ_NEED
self.trans_total = [0, 0, 0]
self.rot_total = [0, 0, 0, 0]
self.seq = self.seq + 1
self.count = 0
if self.seq >= self.MAX_SEQ_NEED:
# ---------------------------------------------------------------------
# The following is going to update the /map->/odom TF frame transform
# https://robot-ros.com/robot/35127.html
# map_odom_tf = tf.transformations.concatenate_matrices(tf.transformations.translation_matrix(
# self.trans_total_seq), tf.transformations.quaternion_matrix(self.rot_total_seq))
# inversed_map_odom_tf = tf.transformations.inverse_matrix(map_odom_tf)
#
# inversed_trans_map_odom = tf.transformations.translation_from_matrix(inversed_map_odom_tf)
# inversed_rot_map_odom = tf.transformations.quaternion_from_matrix(inversed_map_odom_tf)
# self.tf_map_to_odom_br_.sendTransform(
# self.trans_total_seq,
# self.rot_total_seq,
# # inversed_trans_map_odom,
# # inversed_rot_map_odom,
# rospy.Time.now(),
# "map",
# "odom"
# )
# rospy.loginfo("Published /map->/odom TF frame transform!")
# rospy.sleep(5)
# ---------------------------------------------------------------------
start_pos = PoseWithCovarianceStamped()
# start_pos = initpose_aruco
# filling header with relevant information
start_pos.header.frame_id = "map"
start_pos.header.seq = self.seq
# start_pos.header.stamp = rospy.Time.now()
# filling payload with relevant information gathered from subscribing
# to initialpose topic published by RVIZ via rostopic echo initialpose
start_pos.pose.pose.position.x = self.trans_total_seq[0]
# rospy.loginfo(self.trans_total[0])
start_pos.pose.pose.position.y = self.trans_total_seq[1]
start_pos.pose.pose.position.z = 0.0
start_pos.pose.pose.orientation.x = self.rot_total_seq[0]
start_pos.pose.pose.orientation.y = self.rot_total_seq[1]
start_pos.pose.pose.orientation.z = self.rot_total_seq[2]
start_pos.pose.pose.orientation.w = self.rot_total_seq[3]
# start_pos.pose.pose.position.x=map_camera_trans[0]
# start_pos.pose.pose.position.y=map_camera_trans[1]
# start_pos.pose.pose.position.z=0.0
# start_pos.pose.pose.orientation.x=map_camera_rot[0]
# start_pos.pose.pose.orientation.y=map_camera_rot[1]
# start_pos.pose.pose.orientation.z=map_camera_rot[2]
# start_pos.pose.pose.orientation.w=map_camera_rot[3]
# start_pos.pose.pose.position.x = 5.0
# start_pos.pose.pose.position.y = 10.0
# start_pos.pose.pose.position.z = 0.0
# start_pos.pose.pose.orientation.x = 0.0
# start_pos.pose.pose.orientation.y = 0.0
# start_pos.pose.pose.orientation.z = -0.694837665627
# start_pos.pose.pose.orientation.w = 0.719166613815
# start_pos.pose.covariance[0] = 0.01
# start_pos.pose.covariance[7] = 0.01
# start_pos.pose.covariance[1:7] = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
# start_pos.pose.covariance[8:34] = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
# 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
# start_pos.pose.covariance[35] = 0.001
# rospy.loginfo(start_pos)
self.initpose_pub.publish(start_pos)
rospy.sleep(1)
self.initpose_pub.publish(start_pos)
# self.trans_total=[0,0,0]
# self.rot_total=[0,0,0,0]
self.IF_GOT_INIT = True
import numpy as np
from nav_msgs.msg import OccupancyGrid
import cv2
rospy.init_node('load_wp')
tfBuffer = tf2_ros.Buffer()
listener = tf2_ros.TransformListener(tfBuffer)
trans = tfBuffer.lookup_transform('map', 'world', rospy.Time(0),
rospy.Duration(1.0))
map_file = rospy.get_param("/load_wp/map_path")
wp_file = rospy.get_param("/load_wp/path_to_save_wp")
text_file = open(map_file, "r")
lines = text_file.readlines()
wp = PoseWithCovariance()
wp_transformed_st = PoseWithCovarianceStamped()
d = 4000
bw_color = 33
img = np.ones((d, d, 1), np.uint8) * bw_color
dx = rospy.get_param("/load_wp/window/x")
dy = rospy.get_param("/load_wp/window/y")
cx = rospy.get_param("/load_wp/changed_origin/x")
cy = rospy.get_param("/load_wp/changed_origin/y")
def readMap_cb(data):
objectmap = data
map_w = data.info.width
map_h = data.info.height
map_scale = data.info.resolution
#!/usr/bin/env python
import rospy
import math
import numpy as np
from geometry_msgs.msg import PoseWithCovarianceStamped
from path_planning.msg import comb
from tf.transformations import quaternion_from_euler
amcl_init_pose = PoseWithCovarianceStamped()
longitude_0 = 113.6938667 * math.pi / 180
latitude_0 = 34.7989201 * math.pi / 180
gps_recovery_x = []
gps_recovery_y = []
gps_recovery_flag = 1
def gps_cb(gpsMsg):
global gps_recovery_flag, latitude_0, longitude_0, heading_0, pub, amcl_init_pose
latitude_tmp = gpsMsg.Latitude
longitude_tmp = gpsMsg.Longitude
gps_recovery_x.append(latitude_tmp)
gps_recovery_y.append(longitude_tmp)
def spin(self):
state_message = PoseWithCovarianceStamped()
# http://docs.ros.org/melodic/api/geometry_msgs/html/msg/PoseWithCovariance.html => row major list 6 x 6 matrix
while not rospy.is_shutdown():
# after receiving the first cmd_vel
if len(self.X_list) != 0:
state_message.header.stamp = rospy.Time.now()
state_message.header.frame_id = "odom_kf"
state_message.pose.pose.position.x = self.X_list[
-1] + self.initial_pose.pose.pose.position.x
state_message.pose.pose.position.y = 0
state_message.pose.pose.position.z = 0
state_message.pose.pose.orientation.x = 0
state_message.pose.pose.orientation.y = 0
state_message.pose.pose.orientation.z = 0
state_message.pose.pose.orientation.w = 1
state_message.pose.covariance[0] = self.P_list[-1]
self.state_publisher.publish(state_message)
self.rate.sleep(
) # publish PoseWithCovarianceStamped msg as per designated Hz
if rospy.get_time(
) - self.time_record_cmd_now > MSG_INTERVAL_TIME:
pose_path_list = []
pose_path_time = []
scalar_X_list = []
scalar_P_list = []
# Task 2 - C: path based on pose and scan
for i in range(len(self.X_list)):
scalar_X_list.append(np.asscalar(self.X_list[i]))
scalar_P_list.append(np.asscalar(self.P_list[i]))
# Task 2 - A: path based on pose
for k in range(len(self.pose_diff_list)):
if k == 0:
pose_path_time.append(self.pose_diff_list[k][0])
pose_path_list.append(self.pose_diff_list[k][1])
else:
pose_path_time.append(self.pose_diff_list[k][0] +
pose_path_time[k - 1])
pose_path_list.append(
self.pose_diff_list[k][1] +
pose_path_list[k - 1]
) # recursively accumulate the pose path
print("finished!")
# Data check on screen
print("pose_path", pose_path_list, "length",
len(pose_path_list))
print("pose_path_time", pose_path_time)
print("X_list", scalar_X_list, "length",
len(scalar_X_list))
print("X_time", self.time_accumulation_scan_list, "length",
len(self.time_accumulation_scan_list))
#print("P_list", scalar_P_list, "length", len(scalar_P_list))
# file save function
csv_data_saver(CSV_SAVE_PATH_POSE, pose_path_time,
pose_path_list)
csv_data_saver(CSV_SAVE_PATH_POSE_ESTIMATE,
self.time_accumulation_scan_list,
scalar_X_list)
rospy.signal_shutdown("finish!")
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)
from nav_msgs.msg import OccupancyGrid
from nav_msgs.srv import GetMap
from ros_enhsp.srv import ProblemGenerator
import rospkg
import rospy
from std_msgs.msg import String
map = OccupancyGrid()
def map_callback(msg):
global map
map = msg
robot_pose = PoseWithCovarianceStamped()
def pose_callback(msg):
global robot_pose
robot_pose = msg
#### Node function ####
def node():
# Initialize node
rospy.init_node('problem_interface', anonymous=True)
# Subscribers
rospy.Subscriber('/map', OccupancyGrid, map_callback)
rospy.Subscriber('/amcl_pose', PoseWithCovarianceStamped, pose_callback)
def __init__(self):
rospy.init_node('surveil', anonymous=True)
rospy.on_shutdown(self.shutdown)
# How long in seconds should the robot pause at each location?
self.rest_time = rospy.get_param("~rest_time", 5)
# Are we running in the fake simulator?
self.fake_test = rospy.get_param("~fake_test", False)
# Goal state return values
goal_states = [
'PENDING', 'ACTIVE', 'PREEMPTED', 'SUCCEEDED', 'ABORTED',
'REJECTED', 'PREEMPTING', 'RECALLING', 'RECALLED', 'LOST'
]
# Set up the goal locations. Poses are defined in the map frame.
# An easy way to find the pose coordinates is to point-and-click
# Nav Goals in RViz when running in the simulator.
# Pose coordinates are then displayed in the terminal
# that was used to launch RViz.
loc = ['A', 'B', 'C', 'D']
locations = dict()
locations['A'] = Pose(Point(9.5, 10.899, 0.000),
Quaternion(0.000, 0.000, 0.594, 0.804))
locations['B'] = Pose(Point(0.699, 3.099, 0.000),
Quaternion(0.000, 0.000, 0.908, -0.418))
locations['C'] = Pose(Point(-2.799, -1.800, 0.000),
Quaternion(0.000, 0.000, -0.825, 0.564))
locations['D'] = Pose(Point(-7.099, -2.890, 0.000),
Quaternion(0.000, 0.000, 0.99, 0.031))
# Publisher to manually control the robot (e.g. to stop it, queue_size=5)
self.cmd_vel_pub = rospy.Publisher('cmd_vel', Twist, queue_size=5)
# Subscribe to the move_base action server
self.move_base = actionlib.SimpleActionClient("move_base",
MoveBaseAction)
rospy.loginfo("Waiting for move_base action server...")
# Wait 60 seconds for the action server to become available
self.move_base.wait_for_server(rospy.Duration(60))
rospy.loginfo("Connected to move base server")
# A variable to hold the initial pose of the robot to be set by
# the user in RViz
initial_pose = PoseWithCovarianceStamped()
# Variables to keep track of success rate, running time,
# and distance traveled
n_locations = len(locations)
n_goals = 0
n_successes = 0
i = 0
distance_traveled = 0
start_time = rospy.Time.now()
running_time = 0
location = ""
last_location = ""
# Get the initial pose from the user
rospy.loginfo(
"*** Click the 2D Pose Estimate button in RViz to set the robot's initial pose..."
)
rospy.wait_for_message('initialpose', PoseWithCovarianceStamped)
self.last_location = Pose()
rospy.Subscriber('initialpose', PoseWithCovarianceStamped,
self.update_initial_pose)
# Make sure we have the initial pose
while initial_pose.header.stamp == "":
rospy.sleep(1)
rospy.loginfo("Starting navigation test")
# Begin the main loop and run through a sequence of locations
while not rospy.is_shutdown():
# Keep track of the distance traveled.
# Use updated initial pose if available.
if initial_pose.header.stamp == "":
distance = sqrt(
pow(
locations[loc[i]].position.x -
locations[loc[il]].position.x, 2) + pow(
locations[loc[i]].position.y -
locations[loc[il]].position.y, 2))
else:
rospy.loginfo("Updating current pose.")
distance = sqrt(
pow(
locations[loc[i]].position.x -
initial_pose.pose.pose.position.x, 2) + pow(
locations[loc[i]].position.y -
initial_pose.pose.pose.position.y, 2))
initial_pose.header.stamp = ""
if i == 3:
i = 0
elif i != 0:
i = i + 1
rospy.loginfo("f**k")
n_goals += 1
# Set up the next goal location
self.goal = MoveBaseGoal()
self.goal.target_pose.pose = locations[loc[i]]
self.goal.target_pose.header.frame_id = 'map'
self.goal.target_pose.header.stamp = rospy.Time.now()
# Let the user know where the robot is going next
rospy.loginfo("Going to: " + str(loc[i]))
# Start the robot toward the next location
self.move_base.send_goal(self.goal)
rospy.loginfo("check" + str(loc[i]))
# Allow 10 minutes to get there
finished_within_time = self.move_base.wait_for_result(
rospy.Duration(300))
rospy.loginfo("done" + str(loc[i]))
# Check for success or failure
if not finished_within_time:
self.move_base.cancel_goal()
rospy.loginfo("Timed out achieving goal")
else:
state = self.move_base.get_state()
if state == GoalStatus.SUCCEEDED:
rospy.loginfo("Goal succeeded!")
n_successes += 1
distance_traveled += distance
rospy.loginfo("State:" + str(state))
else:
rospy.loginfo("Goal failed with error code: " +
str(goal_states[state]))
# How long have we been running?
running_time = rospy.Time.now() - start_time
running_time = running_time.secs / 60.0
# Print a summary success/failure, distance traveled and time elapsed
rospy.loginfo("Success so far: " + str(n_successes) + "/" +
str(n_goals) + " = " +
str(100 * n_successes / n_goals) + "%")
rospy.loginfo("Running time: " + str(trunc(running_time, 1)) +
" min Distance: " +
str(trunc(distance_traveled, 1)) + " m")
rospy.sleep(self.rest_time)
