def update_obstacle_viz(self, inmsg):
msg = PoseArray()
msg.header = Header()
# since the obstacles are derived from the laser, specify that frame id
msg.header.frame_id = "laser"
msg.poses = [Pose(o.center, None) for o in inmsg.obstacles]
self.obstacle_poses_pub.publish(msg)
def processFeedback(feedback):
(frame_transform_pos, frame_transform_rot) = tf_listener.lookupTransform(
lleg_end_coords, feedback.header.frame_id, rospy.Time(0.0))
frame_pose = concatenate_matrices(translation_matrix(frame_transform_pos),
quaternion_matrix(frame_transform_rot))
center_local_pose = poseMsgToMatrix(feedback.pose)
left_local_offset = translation_matrix([0, foot_margin / 2.0, 0])
left_local_pose = concatenate_matrices(center_local_pose, left_local_offset)
right_local_offset = translation_matrix([0, - foot_margin / 2.0, 0])
right_local_pose = concatenate_matrices(center_local_pose,
right_local_offset)
left_global_pose = concatenate_matrices(frame_pose, left_local_pose)
right_global_pose = concatenate_matrices(frame_pose, right_local_pose)
footsteps = PoseArray()
footsteps.header.frame_id = lleg_end_coords
footsteps.header.stamp = feedback.header.stamp
footsteps.poses = [poseMatrixToMsg(right_global_pose),
poseMatrixToMsg(left_global_pose)]
pub_debug_current_pose_array.publish(footsteps)
if feedback.menu_entry_id == 0:
return # do nothing
elif feedback.menu_entry_id == 1: # reset to origin
server.setPose(feedback.marker_name, poseMatrixToMsg(identity_matrix()))
server.applyChanges()
return
elif feedback.menu_entry_id == 2: # reset orientation to origin
position = [feedback.pose.position.x,
feedback.pose.position.y,
feedback.pose.position.z]
server.setPose(feedback.marker_name, poseMatrixToMsg(translation_matrix(position)))
server.applyChanges()
return
elif feedback.menu_entry_id == 3: # execute
pub_goal.publish(footsteps)
class PathSerializer():
# Must have __init__(self) function for a class, similar to a C++ class constructor.
def __init__(self):
# Get the ~private namespace parameters from command li
self.poseArray = PoseArray()
self.filename = "/home/shart/marker_trajectories/hose/hose_traj_1.txt"
self.yaml = "/home/shart/marker_trajectories/hose/hose_traj_1.yaml"
def setFile(self, fname) :
self.filename = fname
self.yaml = fname[0:fname.find('.txt')] + ".yaml"
def appendPose(self, data) :
print "got new pose in frame: ", data.header.frame_id
self.poseArray.header.frame_id = data.header.frame_id
self.poseArray.poses.append(data.pose)
foo = StringIO()
self.poseArray.serialize(foo)
with open(self.yaml, 'a') as f:
f.write(str(data))
with open(self.filename, 'w') as f:
f.write(foo.getvalue())
def publish_poses_grasps(grasps):
rospy.loginfo("Publishing PoseArray on /grasp_pose_from_block_bla for grasp_pose")
grasp_publisher = rospy.Publisher("grasp_pose_from_block_bla", PoseArray)
graspmsg = Grasp()
grasp_PA = PoseArray()
header = Header()
header.frame_id = "base_link"
header.stamp = rospy.Time.now()
grasp_PA.header = header
for graspmsg in grasps:
print graspmsg
print type(graspmsg)
p = Pose(graspmsg.grasp_pose.pose.position, graspmsg.grasp_pose.pose.orientation)
grasp_PA.poses.append(p)
grasp_publisher.publish(grasp_PA)
rospy.loginfo("Press a to continue...")
while True:
choice = raw_input("> ")
if choice == 'a' :
print "Continuing, a pressed"
break
else:
grasp_publisher.publish(grasp_PA)
rospy.sleep(0.1)
def cb(msg):
global p
array = PoseArray()
array.header = msg.header
for footstep in msg.footsteps:
array.poses.append(footstep.pose)
p.publish(array)
def main():
arg_fmt = argparse.RawDescriptionHelpFormatter
parser = argparse.ArgumentParser(formatter_class=arg_fmt,
description=main.__doc__)
required = parser.add_argument_group('required arguments')
required.add_argument(
'-l', '--limb', required=True, choices=['left', 'right'],
help='send joint trajectory to which limb'
)
args = parser.parse_args(rospy.myargv()[1:])
limb = args.limb
rospy.init_node("estimate_depth")
de = DepthEstimator(limb)
de.subscribe()
de.publish()
print "subscribed"
rate = rospy.Rate(100)
while not rospy.is_shutdown():
if len(de.goal_poses) > 0:
print de.goal_poses
pose_msg = PoseArray()
pose_msg.poses = de.goal_poses
de.handler_pub.publish(pose_msg)
rate.sleep()
def run(self):
r = rospy.Rate(5)
while not rospy.is_shutdown():
# Publish centroid for visualization
my_point = PointStamped()
my_point.header.frame_id = 'STAR'
my_point.point.x = self.centroid[0]
my_point.point.y = self.centroid[1]
self.centroid_pub.publish(my_point)
# Publish arena bounds for visualization
my_polygon = PolygonStamped()
my_polygon.header.frame_id = 'STAR'
my_polygon.polygon.points = [
Point32(x=-0.87, y=0.53, z=0),
Point32(x=-0.87, y=-3.73, z=0),
Point32(x=2.2, y=-3.73, z=0),
Point32(x=2.2, y=0.53, z=0),
]
self.arena_pub.publish(my_polygon)
# Publish robot poses for visualization
my_posearray = PoseArray()
my_posearray.header.frame_id = 'STAR'
my_posearray.poses = [p for p in self.bot_pos if p is not None]
self.robot_poses_pub.publish(my_posearray)
# If we have received a pose for all robots, start sending packets
if not None in self.bot_pos:
self.bot_pos_copy = deepcopy(self.bot_pos)
for i in range(len(self.bot_pos)):
self.send_pkt(i)
r.sleep()
def update(self): # protected region updateCode on begin # in_CameraDetections = copy.deepcopy(self.in_CameraDetections) # check if detection is available if len(in_CameraDetections.poses) <= 0: return # do transformation from pixel coords to camera_base_link coords in meter out1_CameraDetections = PoseArray() out1_CameraDetections.header = in_CameraDetections.header for pose in in_CameraDetections.poses: new_pose = Pose() new_pose.position.x = (pose.position.x - self.config_ResolutionX/2.0) * self.config_MeterPerPixel new_pose.position.y = (pose.position.y - self.config_ResolutionY/2.0) * self.config_MeterPerPixel new_pose.position.z = 0.0 new_pose.orientation = pose.orientation out1_CameraDetections.poses.append(new_pose) # do transformation from camera_base_link to robot base_link out_CameraDetections = PoseArray() out_CameraDetections.header = out1_CameraDetections.header for pose in out1_CameraDetections.poses: new_pose = Pose() new_pose.position.x = self.camera_base_link_offset_X + pose.position.x new_pose.position.y = self.camera_base_link_offset_Y - pose.position.y new_pose.position.z = 0.0 new_pose.orientation = pose.orientation # TODO: rotate 180deg around x-axis out_CameraDetections.poses.append(new_pose) self.out_CameraDetections = out_CameraDetections # protected region updateCode end # pass
def main():
arg_fmt = argparse.RawDescriptionHelpFormatter
parser = argparse.ArgumentParser(formatter_class=arg_fmt,
description=main.__doc__)
required = parser.add_argument_group('required arguments')
required.add_argument(
'-l', '--limb', required=False, choices=['left', 'right'],
help='send joint trajectory to which limb'
)
args = parser.parse_args(rospy.myargv()[1:])
if args.limb is None:
limb = 'right'
else:
limb = args.limb
rospy.init_node("get_goal_poses")
pose_calc = PoseCalculator(limb)
pose_calc.subscribe()
pose_calc.publish()
rate = rospy.Rate(params['rate'])
while not rospy.is_shutdown():
if len(pose_calc.goal_poses) > 0:
print pose_calc.goal_poses
pose_msg = PoseArray()
pose_msg.poses = pose_calc.goal_poses
pose_calc.handler_pub.publish(pose_msg)
rate.sleep()
def publish_poses(pose_array):
pub = rospy.Publisher('adept_wrist_orientation', PoseArray, queue_size=10)
pose_msg = PoseArray()
pose_msg.poses = pose_array
pose_msg.header.stamp = rospy.Time.now()
pose_msg.header.frame_id = "/world"
pub.publish(pose_msg)
def newMessageReceived(detectedPersons):
poseArray = PoseArray()
poseArray.header = detectedPersons.header
for detectedPerson in detectedPersons.detections:
poseArray.poses.append(detectedPerson.pose.pose)
pub.publish(poseArray)
def publish_particles(self):
pose_arr = PoseArray()
pose_arr.header.stamp = rospy.Time.now()
pose_arr.header.frame_id = 'map'
pose_arr.poses = []
for p in self.particle_ls:
pose_arr.poses.append(p.pose)
self.particle_cloud_pub.publish(pose_arr)
def facecb(data):
if len(data.faces) > 0:
pa = PoseArray()
pa.header = data.header
for face in data.faces:
print ".",
face_pose = Pose()
face_pose.position = face.position
face_pose.orientation.w = 1.0
pa.poses.append(face_pose)
face_pub.publish(pa)
def processFeedback(feedback):
(frame_transform_pos, frame_transform_rot) = tf_listener.lookupTransform(
lleg_end_coords, feedback.header.frame_id, rospy.Time(0.0))
frame_pose = concatenate_matrices(translation_matrix(frame_transform_pos),
quaternion_matrix(frame_transform_rot))
center_local_pose = poseMsgToMatrix(feedback.pose)
center_local_pos = translation_from_matrix(center_local_pose)
left_local_offset = translation_matrix([0, foot_margin / 2.0, 0])
left_local_pose = concatenate_matrices(center_local_pose, left_local_offset)
right_local_offset = translation_matrix([0, - foot_margin / 2.0, 0])
right_local_pose = concatenate_matrices(center_local_pose,
right_local_offset)
left_global_pose = concatenate_matrices(frame_pose, left_local_pose)
right_global_pose = concatenate_matrices(frame_pose, right_local_pose)
left_global_pos = translation_from_matrix(left_global_pose)
right_global_pos = translation_from_matrix(right_global_pose)
footsteps = PoseArray()
footsteps.header.frame_id = lleg_end_coords
footsteps.header.stamp = feedback.header.stamp
footsteps.poses = [poseMatrixToMsg(right_global_pose),
poseMatrixToMsg(left_global_pose)]
pub_debug_current_pose_array.publish(footsteps)
# check distance
need_to_fix = False
if (feedback.event_type == InteractiveMarkerFeedback.MOUSE_UP and
(abs(center_local_pos[0]) + 0.01> max_x or
abs(center_local_pos[2]) + 0.01> max_z or
center_local_pos[2] - 0.01 < 0)):
if abs(center_local_pos[0]) > max_x:
center_local_pos[0] = max_x * center_local_pos[0] / abs(center_local_pos[0])
if center_local_pos[2] < 0:
center_local_pos[2] = 0
elif abs(center_local_pos[2]) > max_z:
center_local_pos[2] = max_z
rospy.logwarn("need to reset")
new_center_pose = translation_matrix(center_local_pos)
server.setPose(feedback.marker_name, poseMatrixToMsg(new_center_pose))
server.applyChanges()
elif feedback.menu_entry_id == 0:
return # do nothing
elif feedback.menu_entry_id == 1: # reset to origin
server.setPose(feedback.marker_name, poseMatrixToMsg(identity_matrix()))
server.applyChanges()
return
elif feedback.menu_entry_id == 2: # reset orientation to origin
position = [feedback.pose.position.x,
feedback.pose.position.y,
feedback.pose.position.z]
server.setPose(feedback.marker_name, poseMatrixToMsg(translation_matrix(position)))
server.applyChanges()
return
elif feedback.menu_entry_id == 3: # execute
pub_goal.publish(footsteps)
def readPoseArrayFromFile(filename):
try:
# read file
with open(filename) as f:
poseArrayString = f.read()
# deserialize it into PoseArray
pa = PoseArray()
pa.deserialize(poseArrayString)
return pa
except:
print "Returning no pose array for " + filename
return None
def transformed_waypoints():
pub = rospy.Publisher("transformed_waypoints", PoseArray, queue_size=10)
rospy.init_node("transformed_waypoints", anonymous=True)
rate = rospy.Rate(1) # 10hz
b = Bagger()
posarr = PoseArray()
posarr.header.frame_id = "r_wrist_roll_link"
posarr.header.stamp = rospy.get_rostime()
posarr.poses = b.getTransformedWaypointsPoses()
while not rospy.is_shutdown():
pub.publish(posarr)
rate.sleep()
def publish_grasps_as_poses(self, grasps):
rospy.loginfo("Publishing PoseArray on /grasp_pose_from_block_bla for grasp_pose")
graspmsg = Grasp()
grasp_PA = PoseArray()
header = Header()
header.frame_id = "base_link"
header.stamp = rospy.Time.now()
grasp_PA.header = header
for graspmsg in grasps:
p = Pose(graspmsg.grasp_pose.pose.position, graspmsg.grasp_pose.pose.orientation)
grasp_PA.poses.append(p)
self.grasp_publisher.publish(grasp_PA)
rospy.sleep(0.1)
def publish_grasps_as_poses(grasps):
grasp_publisher = rospy.Publisher(GRASP_POSES_TOPIC, PoseArray)
rospy.loginfo("Publishing PoseArray on " + GRASP_POSES_TOPIC + " representing grasps as poses.")
#graspmsg = Grasp()
grasp_PA = PoseArray()
header = Header()
header.frame_id = "base_link"
header.stamp = rospy.Time.now()
grasp_PA.header = header
for graspmsg in grasps:
p = Pose(graspmsg.grasp_pose.pose.position, graspmsg.grasp_pose.pose.orientation)
grasp_PA.poses.append(p)
grasp_publisher.publish(grasp_PA)
rospy.sleep(0.1)
def main():
rospy.init_node('display_grasps',anonymous=True)
parser = argparse.ArgumentParser()
parser.add_argument("name")
parser.add_argument("selection",nargs='*',type=int)
parser.add_argument('--frame')
myargv = rospy.myargv()
del myargv[0]
args = parser.parse_args(myargv)
print args
pub = rospy.Publisher('grasp_poses',PoseArray)
name = args.name
print name
rospy.loginfo('loading object %s', name)
ref_pc_dir = roslib.packages.get_pkg_subdir('google_goggles','data/points/ref')
filename = os.path.join(ref_pc_dir, name + '.bag')
bag = rosbag.Bag(filename)
grasps = None
grasp_poses = None
for topic, msg, t in bag.read_messages():
if msg._type == 'google_goggles_msgs/ObjectReferenceData':
rospy.loginfo('Got reference data on topic %s',topic)
grasps = msg.grasps
elif msg._type == 'geometry_msgs/PoseArray':
grasp_poses = msg
if grasps:
grasp_poses = PoseArray()
grasp_poses.poses = [grasp.grasp_pose for grasp in grasps]
print [grasp.success_probability for grasp in grasps]
if args.selection:
grasp_poses.poses = [grasp_poses.poses[i] for i in args.selection]
grasp_poses.header.stamp = rospy.Time.now()
if args.frame:
grasp_poses.header.frame_id = args.frame
rospy.loginfo('Publishing %d poses in frame %s',len(grasp_poses.poses),grasp_poses.header.frame_id)
pub.publish(grasp_poses)
def smudge_amcl(self, resampledPoses): cont = True pArray = PoseArray() if len(resampledPoses): temp = [] val = Pose() count = 0 # TEST this value, rounding scalar scale = 0.75 while cont: temp = [] val = resampledPoses[0] count = 0 for i in range(0, len(resampledPoses)): if (resampledPoses[i] == val): count = count + 1 else: temp.append(resampledPoses[i]) resampledPoses = temp if (len(resampledPoses) == 0): cont = False # THIS NEEDS TESTS, look at scalar above if (count > 4) and len(resampledPoses) >= 50: #TEST #count = count - 2 count = int(count * scale) for i in range(0, count): if i > 0: newPose = Pose() newPose.position.x = random.gauss(val.position.x, 0.15) #TEST THIS newPose.position.y = random.gauss(val.position.y, 0.15) #TEST THIS newPose.orientation = rotateQuaternion(val.orientation, random.vonmisesvariate(0, 7)) #TEST THIS pArray.poses.append(newPose) else: pArray.poses.append(val) else: pArray.poses = [] return pArray
def publish_place_as_pose(self, places):
if self.DEBUG is 1:
print "[DEBUG] Publishing PoseArray on /place_pose"
place_pose = PoseArray()
# header
header = Header()
header.frame_id = REFERENCE_FRAME
header.stamp = rospy.Time.now()
place_pose.header = header
for place_location in places:
# append position
p = Pose(place_location.place_pose.pose.position, place_location.place_pose.pose.orientation)
place_pose.poses.append(copy.deepcopy(p))
# publish
self.place_publisher.publish(place_pose)
if self.DEBUG is 1:
print '[DEBUG] Published ' + str(len(place_pose.poses)) + ' poses'
def broadcast_objects(self, image_cv):
image_cv, results = self.detector.find_objects(image_cv)
poses = []
for result in results:
center, (width,height), rotation = result
pos = Point(*self.pixel_to_base_link(image_cv, center))
pos.z += 0.04
#q = Quaternion(x=rotation) # placeholder
poses.append( Pose(pos, Quaternion(1, 0, 0, 0) )) # placeholder)
stamped_poses = PoseArray()
stamped_poses.header.frame_id = "base_link"
stamped_poses.header.stamp = rospy.Time(0)
stamped_poses.poses = poses
self.pub_poses.publish(stamped_poses)
return image_cv
def publish_grasps_as_poses(self, grasps):
if self.DEBUG is 1:
print "[DEBUG] Publishing PoseArray on /grasp_pose for grasp_pose"
pa = PoseArray()
# header
header = Header()
header.frame_id = REFERENCE_FRAME
header.stamp = rospy.Time.now()
pa.header = header
# append all poses to pose array
for msg in grasps:
p = Pose(msg.grasp_pose.pose.position, msg.grasp_pose.pose.orientation)
pa.poses.append(p)
# publish
self.grasp_publisher.publish(pa)
if self.DEBUG is 1:
print '[DEBUG] Published ' + str(len(pa.poses)) + ' poses'
rospy.sleep(2)
def __init__(self):
# Get the ~private namespace parameters from command li
self.poseArray = PoseArray()
self.waypointArray = PoseArray()
self.filename = "/home/shart/marker_trajectories/hose/hose_waypoints_1.txt"
self.pathPub = rospy.Publisher("/path_publisher/path", Path)
self.posePub = rospy.Publisher("/path_publisher/poseArray", PoseArray)
self.waypointPub = rospy.Publisher("/path_publisher/waypointArray", PoseArray)
self.num_points = 10
self.frameOffset = Frame()
def main():
rospy.init_node('test_linear_move')
touch_pub = rospy.Publisher('touch_pose', PoseStamped)
touch_arr_pub = rospy.Publisher('touch_poses', PoseArray)
args = sys.argv
tf_listener = tf.TransformListener()
dm_client = actionlib.SimpleActionClient(args[1] + '_epc_move/direct_move',
EPCDirectMoveAction)
dm_client.wait_for_server()
lm_client = actionlib.SimpleActionClient(args[1] + '_epc_move/linear_move',
EPCLinearMoveAction)
lm_client.wait_for_server()
rospy.sleep(5)
param_bounds = [(0.76, 1.03),
(-0.05, -0.41),
(-0.30, 0.32),
(-60.0, 30.0),
(-20.0, 45.0)]
touch_arr = PoseArray()
while not rospy.is_shutdown():
params = []
for pmin, pmax in param_bounds:
params.append(np.random.uniform(pmin, pmax))
pos = params[:3]
quat = tf_trans.quaternion_from_euler(np.radians(params[3]), np.radians(params[4]),
0, 'rzyx')
touch_pose = PoseStamped()
touch_pose.header.frame_id = 'torso_lift_link'
touch_pose.header.stamp = rospy.Time.now()
touch_pose.pose = Pose(Point(*pos), Quaternion(*quat))
touch_pub.publish(touch_pose)
touch_arr.header = touch_pose.header
touch_arr.poses.append(touch_pose.pose)
touch_arr_pub.publish(touch_arr)
print touch_pose
tool_frame = args[1] + '_scratcher_tip'
dm_goal = EPCDirectMoveGoal(touch_pose, tool_frame, True, 0.02, 0.35, 0.2, 1.0, 5)
dm_client.send_goal(dm_goal)
rospy.sleep(1)
def compute_path(self, req):
self.start.x = req.start.pose.position.x
self.start.y = req.start.pose.position.y
self.final.x = req.final.pose.position.x
self.final.y = req.final.pose.position.y
self.marker_rrt_edges = Marker(id=0,
header=Header(frame_id='map',
stamp=rospy.Time.now()))
self.marker_rrt_edges.type = Marker.LINE_LIST
self.marker_rrt_edges.action = Marker.ADD
self.marker_rrt_edges.scale.x = 0.04
# marker color
self.line_color = ColorRGBA()
self.line_color.a = 0.5
self.line_color.r = 1.0
res = path_calcResponse()
res.result = 0
if self.compute_rrt() == False:
return res
path = [[self.final.x, self.final.y]]
lastIndex = len(self.nodes) - 1
while self.nodes[lastIndex].parent is not None:
node = self.nodes[lastIndex]
path.append([node.x, node.y])
lastIndex = node.parent
path.append([self.start.x, self.start.y])
path.reverse()
new_path = path
#new_path = self.path_smoothing(path,1000)
poseArray = PoseArray()
#path_a = Path()
for p in new_path:
pose = Pose()
# pose_stamp=PoseStamped()
# pose_stamp.pose.position.x = p[0]
# pose_stamp.pose.position.y = p[1]
pose.position.x = p[0]
pose.position.y = p[1]
# path_a.poses.append(pose_stamp)
poseArray.poses.append(pose)
#rrt_path_publisher.publish(path_a)
res.path = poseArray
print path
res.result = 1
return res
def __init__(self):
self.people = People()
self.angles = PoseArray()
self.pub = rospy.Publisher('/people_yolo_detector',
People,
queue_size=10)
self.pub2 = rospy.Publisher('/people_yolo_angles',
PoseArray,
queue_size=1)
self.sub = rospy.Subscriber("/darknet_ros/bounding_boxes",
BoundingBoxes, self.callback)
def function_pub_cart_path_elfin5(self):
pa = PoseArray()
pa.header.stamp = rospy.get_rostime()
pa.header.frame_id = 'elfin_base_link'
ps = Pose()
ps.position.x = 0.13
ps.position.y = -0.1
ps.position.z = 1.008
ps.orientation.x = 0.295
ps.orientation.y = 0
ps.orientation.z = 0
ps.orientation.w = 0.956
pa.poses.append(ps)
self.cart_path_pub.publish(pa)
def __init__(self):
self.transformer = tf_ros.TransformListener()
self.publisher = rospy.Publisher('estimated_pose',
PoseArray,
queue_size=1)
self.pose_publisher = rospy.Publisher('estimate_pose',
Pose,
queue_size=1)
self.location_array = PoseArray()
self.location_array.header.frame_id = "odom"
self.lock = Lock()
self.counter = 0
def recv_boat_info():
global number_boats
responses = []
for i in range(number_boats):
service = '/asv' + str(i + 1) + '/asv_service'
print service
rospy.wait_for_service(service)
try:
boat_info_rec_service = rospy.ServiceProxy(
'/asv' + str(i + 1) + '/asv_service', BoatInfo)
response = boat_info_rec_service(Pose(), PoseArray())
responses.append(response)
except rospy.ServiceException, e:
print "Service call failed: %s" % e
def pub_teststates():
pub = rospy.Publisher('pub_teststates', PoseArray, queue_size=10)
rospy.init_node('pub_teststates', anonymous=True)
rate = rospy.Rate(1) # 10hz
a = Bagger(filename=constants.TEST_FILE)
markers = a.getMarkers()
start_m = markers[0]
goal_m = markers[-1]
sg_markers = [start_m, goal_m]
b = Bagger(filename=constants.DATA_FILE)
# tranformed_sg_markers = b.transformStartGoal(sg_markers)
posarr = PoseArray()
posarr.header.frame_id = 'r_wrist_roll_link'
posarr.header.stamp = rospy.get_rostime()
posarr.poses = b.transformStartGoal(sg_markers)
while not rospy.is_shutdown():
pub.publish(posarr)
rate.sleep()
print "publishing transformed_start_goal"
def publish_particle_viz(self):
"""
Publish a visualization of self.particle_cloud for use in rviz.
"""
self.particle_pub.publish(
PoseArray(
header=Header(
stamp=rospy.Time.now(),
frame_id=self.map_frame),
poses=[
p.as_pose() for p in self.particle_cloud]))
if self.debug:
print("Publishing new visualization.")
def _publish_places(self, places):
"""
Publish places as poses, using a PoseArray message
"""
if self._places_pub.get_num_connections() > 0:
msg = PoseArray()
msg.header.frame_id = self._robot.get_planning_frame()
msg.header.stamp = rospy.Time.now()
for place in places:
msg.poses.append(place.place_pose.pose)
self._places_pub.publish(msg)
def function_pub_cart_path_elfin10(self):
pa = PoseArray()
pa.header.stamp = rospy.get_rostime()
pa.header.frame_id = 'elfin_base_link'
ps = Pose()
ps.position.x = 0.097
ps.position.y = -0.124
ps.position.z = 1.226
ps.orientation.x = 0.317
ps.orientation.y = 0
ps.orientation.z = 0
ps.orientation.w = 0.948
pa.poses.append(ps)
self.cart_path_pub.publish(pa)
def function_pub_cart_path_elfin3(self):
pa = PoseArray()
pa.header.stamp = rospy.get_rostime()
pa.header.frame_id = 'elfin_base_link'
ps = Pose()
ps.position.x = 0.215
ps.position.y = 0.214
ps.position.z = 0.719
ps.orientation.x = 0
ps.orientation.y = 0
ps.orientation.z = 0
ps.orientation.w = 1
pa.poses.append(ps)
self.cart_path_pub.publish(pa)
def publishParticles(self):
"""
Function responsible for publishing the particles
"""
# http://docs.ros.org/en/melodic/api/geometry_msgs/html/msg/PoseArray.html
# http://docs.ros.org/en/diamondback/api/geometry_msgs/html/msg/PoseArray.html
# https://answers.ros.org/question/60209/what-is-the-proper-way-to-create-a-header-with-python/
# https://www.programcreek.com/python/example/86351/std_msgs.msg.Header
particleCloud = PoseArray()
# Create PoseArray header
particleCloud.header = std_msgs.msg.Header()
particleCloud.header.stamp = rospy.Time.now()
particleCloud.header.frame_id = "map"
particlePoses = []
for i in range(self.numParticles):
pose = Pose()
pose.position.x = self.particlesPose[i, 0]
pose.position.y = self.particlesPose[i, 1]
# https://answers.ros.org/question/69754/quaternion-transformations-in-python/
quaternion = tf.transformations.quaternion_from_euler(
0, 0, self.particlesPose[i, 2])
pose.orientation.x = quaternion[0]
pose.orientation.y = quaternion[1]
pose.orientation.z = quaternion[2]
pose.orientation.w = quaternion[3]
particlePoses.append(pose)
particleCloud.poses = particlePoses
self.particlePublisher.publish(particleCloud)
def get_eef_poses_from_trajectory(self, trajectory):
"""
For a RobotTrajectory, determine the end-effector pose at each
time step along the trajectory.
TODO: test this function
"""
# Create the output array
poses_out = PoseArray()
poses_out.header.seq = copy.deepcopy(
trajectory.joint_trajectory.header.seq)
poses_out.header.stamp = rospy.Time.now()
poses_out.header.frame_id = 1
# Loop setup
poses_tot = len(trajectory.joint_trajectory.points)
pose_list = [None] * poses_tot
joint_names = trajectory.joint_trajectory.joint_names
# Build array of 'PoseStamped' waypoints
for i in range(0, poses_tot):
# Get the pose using FK
joint_states = trajectory.joint_trajectory.points[i].positions
time = trajectory.joint_trajectory.points[i].time_from_start
resp = self.fk_solve(joint_states, joint_names)
# Put the pose into list
pose.header.seq = i
pose.header.stamp = time
pose_list[i] = resp.pose_stamped
# Put generated list into the PoseArray
poses_out.poses = pose_list
return poses_out
def pub(self):
particle_pose = PoseArray()
particle_pose.header.frame_id = 'map'
particle_pose.header.stamp = rospy.Time.now()
particle_pose.poses = []
estimated_pose = PoseWithCovarianceStamped()
estimated_pose.header.frame_id = 'map'
estimated_pose.header.stamp = rospy.Time.now()
estimated_pose.pose.pose.position.x = np.mean(self.particles[:, 0])
estimated_pose.pose.pose.position.y = np.mean(self.particles[:, 1])
estimated_pose.pose.pose.position.z = 0.0
quaternion = tf_conversions.transformations.quaternion_from_euler(
0, 0, np.mean(self.particles[:, 2]))
estimated_pose.pose.pose.orientation = geometry_msgs.msg.Quaternion(
*quaternion)
for ii in range(self.Np):
pose = geometry_msgs.msg.Pose()
point_P = (self.particles[ii, 0], self.particles[ii, 1], 0.0)
pose.position = geometry_msgs.msg.Point(*point_P)
quaternion = tf_conversions.transformations.quaternion_from_euler(
0, 0, self.particles[ii, 2])
pose.orientation = geometry_msgs.msg.Quaternion(*quaternion)
particle_pose.poses.append(pose)
self.pub_particlecloud.publish(particle_pose)
self.pub_estimated_pos.publish(estimated_pose)
#print(self.laser_frame)
self.laser_tf_br.sendTransform(
(np.mean(self.particles[:, 0]), np.mean(self.particles[:, 1]), 0),
(estimated_pose.pose.pose.orientation.x,
estimated_pose.pose.pose.orientation.y,
estimated_pose.pose.pose.orientation.z,
estimated_pose.pose.pose.orientation.w), rospy.Time.now(),
self.laser_frame, "map")
def callback(self, pose_array):
"""
Convert pose of tag in camera frame to pose of robot
in map frame.
"""
with self._lock:
pose_array_msg = PoseArray()
# Camera frame to tag frame(s)
if (len(pose_array.detections) == 0):
self._pose_detections = None
self._tag_pose_pub.publish(pose_array_msg)
return
pose_detections = np.zeros((len(pose_array.detections), 3))
for i in range(len(pose_array.detections)):
pose_msg = Pose()
tag_id = pose_array.detections[i].id
pose_cam2tag = pose_array.detections[i].pose.pose
poselist_cam2tag = pose2poselist(pose_cam2tag)
poselist_base2tag = transformPose(self._lr, poselist_cam2tag,
'camera', 'base_link')
poselist_tag2base = invPoselist(poselist_base2tag)
poselist_map2base = transformPose(self._lr, poselist_tag2base,
'apriltag_' + str(tag_id),
'map')
pubFrame(self._br,
pose=poselist_map2base,
frame_id='/base_link',
parent_frame_id='/map')
robot_pose3d = lookupTransform(self._lr, '/map', '/base_link')
robot_position2d = robot_pose3d[0:2]
robot_yaw = tf.transformations.euler_from_quaternion(
robot_pose3d[3:7])[2]
robot_pose2d = robot_position2d + [robot_yaw]
pose_detections[i] = np.array(robot_pose2d)
pose_msg.position.x = robot_pose3d[0]
pose_msg.position.y = robot_pose3d[1]
pose_msg.orientation.x = robot_pose3d[3]
pose_msg.orientation.y = robot_pose3d[4]
pose_msg.orientation.z = robot_pose3d[5]
pose_msg.orientation.w = robot_pose3d[6]
pose_array_msg.poses.append(pose_msg)
self._tag_pose_pub.publish(pose_array_msg)
self._pose_detections = pose_detections
def __init__(self, width, height, division):
# set up this python class as ros node and initialize publisher and subscriber
rospy.init_node('crowd_model')
rospy.Subscriber("crowd_pose", PoseArray, self.crowd_data)
rospy.Subscriber("base_scan", LaserScan, self.laser_data)
rospy.Subscriber("pose", PoseStamped, self.pose_data)
self.pub_crowd_density = rospy.Publisher('crowd_density', OccupancyGrid, queue_size=1)
self.pub_crowd_u = rospy.Publisher('crowd_u', MarkerArray, queue_size=1)
self.pub_crowd_d = rospy.Publisher('crowd_d', MarkerArray, queue_size=1)
self.pub_crowd_l = rospy.Publisher('crowd_l', MarkerArray, queue_size=1)
self.pub_crowd_r = rospy.Publisher('crowd_r', MarkerArray, queue_size=1)
self.pub_crowd_ur = rospy.Publisher('crowd_ur', MarkerArray, queue_size=1)
self.pub_crowd_ul = rospy.Publisher('crowd_ul', MarkerArray, queue_size=1)
self.pub_crowd_dr = rospy.Publisher('crowd_dr', MarkerArray, queue_size=1)
self.pub_crowd_dl = rospy.Publisher('crowd_dl', MarkerArray, queue_size=1)
self.pub_crowd_model = rospy.Publisher('crowd_model', CrowdModel, queue_size=1)
self.pub_visibility_grid = rospy.Publisher('visibility_grid', OccupancyGrid, queue_size=1)
# intialize time
self.prev_message_time = rospy.Time.now()
self.message_time = rospy.Time.now()
self.init_time = rospy.Time.now()
self.time = (self.message_time - self.init_time)
# sensor data (laser scan, robot pose, crowd poses)
self.crowd_poses = PoseArray()
# robot current pose is x,y,theta
self.robot_pose = [0,0,0]
self.laser_scan_endpoints = []
# indicates if the robot has access to the crowd information in that grid
self.is_grid_active = [[False for x in range(division)] for y in range(division)]
self.crowd_count = [[0 for x in range(division)] for y in range(division)]
self.crowd_u = [[0 for x in range(division)] for y in range(division)]
self.crowd_d = [[0 for x in range(division)] for y in range(division)]
self.crowd_l = [[0 for x in range(division)] for y in range(division)]
self.crowd_r = [[0 for x in range(division)] for y in range(division)]
self.crowd_ul = [[0 for x in range(division)] for y in range(division)]
self.crowd_ur = [[0 for x in range(division)] for y in range(division)]
self.crowd_dl = [[0 for x in range(division)] for y in range(division)]
self.crowd_dr = [[0 for x in range(division)] for y in range(division)]
self.crowd_observations = [[1 for x in range(division)] for y in range(division)]
# map details
self.width = width # width of the map
self.height = height # height of the map
self.division = division # number of positions in the map where the model has to predict
self.alpha = 0.7
def __init__(self, whicharm):
self.whicharm = whicharm
self.robot_state = pr2_control_utilities.RobotState()
self.joint_controller = pr2_control_utilities.PR2JointMover(
self.robot_state)
self.planner = pr2_control_utilities.PR2MoveArm(self.joint_controller)
self.server = InteractiveMarkerServer("~trajectory_markers")
self.tf_listener = self.planner.tf_listener
self.visualizer_pub = rospy.Publisher("~trajectory_markers_path",
MarkerArray)
self.trajectory_pub = rospy.Publisher("~trajectory_poses", PoseArray)
rospy.Subscriber("~overwrite_trajectory", PoseArray,
self.overwrite_trajectory)
rospy.Service("~execute_trajectory", Empty,
self.execute_trajectory_srv)
# create an interactive marker for our server
int_marker = InteractiveMarker()
int_marker.header.frame_id = "/base_link"
int_marker.pose.position.x = 0.5
int_marker.pose.position.z = 1.0
int_marker.name = "move_" + whicharm + "_arm"
int_marker.description = "Move the " + whicharm + " arm"
int_marker.scale = 0.2
self.server.insert(int_marker, self.main_callback)
# create the main marker shape
#color = (1,0,0,1)
color = None
self.gripper_marker = utils.makeGripperMarker(color=color)
int_marker.controls.append(self.gripper_marker)
#add the controls
utils.make_6DOF_marker(int_marker)
self.int_marker = int_marker
self.create_menu()
self.server.applyChanges()
self.trajectory = PoseArray()
self.trajectory.header.frame_id = "/base_link"
self.last_gripper_pose = None
if whicharm == "right":
self.ik_utils = self.planner.right_ik
else:
self.ik_utils = self.planner.left_ik
rospy.loginfo("PR2TrajectoryMarkers (%s) is ready", whicharm)
def publish_pose(self, header, results):
msg = PoseArray(header=header)
for r in results:
pose = r["face_origin"]
ori = r["pose"]
quat = T.quaternion_from_euler(ori[2], ori[1], -ori[0])
quat = T.quaternion_multiply(
quat,
T.quaternion_from_euler(np.pi/2., np.pi/2., 0))
pose.orientation.x = quat[0]
pose.orientation.y = quat[1]
pose.orientation.z = quat[2]
pose.orientation.w = quat[3]
msg.poses.append(pose)
self.pub_pose.publish(msg)
def publish_real_points(self, real_points):
poses = PoseArray()
poses.header.stamp = rospy.Time.now()
poses.header.frame_id = "map"
for p in real_points:
x = p[0]
y = p[1]
point = np.array([x, y, 0.0]).T
point = cvt_local2global(point, self.pf.res)
point = Point(point[0] / 1000, point[1] / 1000, 0)
direction = 0.0
quat = Quaternion(
*tf.transformations.quaternion_from_euler(0, 0, direction))
poses.poses.append(Pose(point, quat))
self.lasbea_pub.publish(poses)
def run(self):
rate = rospy.Rate(self.rate)
while not rospy.is_shutdown():
with self.lock:
self._update()
self._crop()
person = self.get_pose()
ppl = PoseArray()
ppl.header.frame_id = '/base_link'
ppl.header.stamp = rospy.Time.now()
ppl.poses.append(person)
self.pub.publish(ppl)
rate.sleep()
def send_trajectory(positions, robot):
'''
SEND_TRAJECTORY Sends a trajectory message to a continuous palpation node
Positions is a list of PyKDL vectors. The current orientation is kept
constant.
'''
msg = PoseArray()
msg.header.stamp = rospy.Time.now()
for idx in range(0, len(positions)):
pose = posemath.toMsg(robot.get_current_position())
pose.position.x = positions[idx].x()
pose.position.y = positions[idx].y()
pose.position.z = positions[idx].z()
msg.poses.append(pose)
posePub.publish(msg)
def send_poses(self, poses, pixels=True):
pose_array = PoseArray()
for p in poses:
pose_x = p[1]
pose_y = p[0]
pose_theta = p[2] + np.pi / 2
if not pixels:
pose_y = p[0] / map_resolution
pose_x = map_img.shape[0] - p[1] / map_resolution
pose_theta = pose_theta
# print(pose_x, pose_y, pose_theta)
pose = build_pose(pose_x, pose_y, pose_theta)
pose_array.poses.append(pose)
pose_array.poses.append(self.goal)
self.path_publisher.publish(pose_array)
def save_to_msg(self):
"""[Convert waypoint into a restorable ROS message. This is done for file saving convenience.]
Returns:
[PoseStamped]: [ros message such that output of this function given to from_msg() would recreate the waypoint]
"""
pose_array = PoseArray()
if self.len() == 0:
return pose_array
for wp in self.get_list():
msg = wp.save_to_msg()
pose_array.header.frame_id = msg.header.frame_id
pose_array.poses.append(msg.pose)
return pose_array
def uniform_sample(self): xs = np.random.uniform(self.top_left_x, self.bottom_right_x, self.num_particles) ys = np.random.uniform(self.bottom_right_y, self.top_left_y, self.num_particles) particles = PoseArray() particles.header.frame_id = "map" particles.header.stamp = rospy.Time.now() for i in range(self.num_particles): pose = Pose() pose.position.x = xs[i] pose.position.y = ys[i] particles.poses.append(pose) self.particle_pub.publish(particles)
def visualize(self):
self.state_lock.acquire()
# (1) Publish tf between map and the laser. Depends on the expected pose.
pose = self.expected_pose()
tf_time = self.publish_tf(pose)
# (2) Publish most recent laser measurement
msg = self.sensor_model.last_laser
msg.header.frame_id = "laser"
msg.header.stamp = tf_time
self.pub_laser.publish(msg)
# (3) Publish PoseStamped message indicating the expected pose of the car.
pose_msg = PoseStamped()
pose_msg.header.seq = self.viz_seq
pose_msg.header.stamp = rospy.Time.now()
pose_msg.header.frame_id = "map"
pose_msg.pose = utils.point_to_pose(pose)
self.pose_pub.publish(pose_msg)
# (4) Publish a subsample of the particles
particle_indices = np.random.choice(np.arange(self.particles.shape[0]),
size=self.MAX_VIZ_PARTICLES,
replace=True,
p=self.weights)
particle_sample = self.particles[particle_indices]
particles_msg = PoseArray()
particles_msg.header.seq = self.viz_seq
particles_msg.header.stamp = rospy.Time.now()
particles_msg.header.frame_id = "map"
particles_msg.poses = [utils.point_to_pose(p) for p in particle_sample]
self.particle_pub.publish(particles_msg)
#rospy.logerr(particles_msg)
self.viz_seq += 1
self.state_lock.release()
def publish(self):
"""Function to publish the particle cloud for visualization."""
cloud_msg = PoseArray()
cloud_msg.header.stamp = rospy.get_rostime()
cloud_msg.header.frame_id = 'map'
cloud_msg.poses = []
for i in range(self.num_particles):
p = Pose()
p.position.x = self.particles[i].x
p.position.y = self.particles[i].y
p.position.z = 0.0
quaternion = tf.transformations.quaternion_from_euler(
0.0, 0.0, self.particles[i].theta)
p.orientation.x = quaternion[0]
p.orientation.y = quaternion[1]
p.orientation.z = quaternion[2]
p.orientation.w = quaternion[3]
cloud_msg.poses.append(p)
self.particles_publisher.publish(cloud_msg)
def __init__(self):
# Create a publisher for acceleration data
self.pub = rospy.Publisher('/Vison/roomba_location_meters',
PoseArray,
queue_size=10)
self.rate = rospy.Rate(10) # needs to be runing fairly fast
# For finding radius
self.a = 391.3
self.b = -0.9371
self.radius = 0
# Altitude
self.alt = 0.8128 # 32 inches ~ as high as the countertop # NEEDS TO CHANGE FROM HARD CODE TO SUBSCRIPTION
#self.alt = 0.3048 # 12 inches ~ as high as the countertop
#self.alt = 1.143 # 45 inches ~ as high as the countertop
self.pose_array = PoseArray()
self.temp_pose = Pose()
self.header = Header()
self.header.seq = 0
self.header.stamp = rospy.Time.now()
self.cap = cv2.VideoCapture(0)
self.roomba_array = PoseArray()
#rospy.Subscriber("mavros/altitude",Altitude,self.altitude_callback)
rospy.Subscriber("mavros/px4flow/ground_distance", Range, self.flownar)
self.Vpub = rospy.Publisher('/Vision/Vllist',
Float64MultiArray,
queue_size=10)
self.Hpub = rospy.Publisher('/Vision/Hllist',
Float64MultiArray,
queue_size=10)
self.Angpub = rospy.Publisher('/Vision/Ang_Hline',
Float64,
queue_size=10)
self.loop_search()
def action_callback(self, goal):
rospy.loginfo('Executing'+" "+str(self._action_name)+"."+"request sent:")
rospy.loginfo(goal)
self.current_param_part_id = goal.param_part_id
res = self.detect_poses(goal.maskCorner, goal.param_part_id)
self.action_result.success = res
if(res):
self.action_result.posesDetected = self.current_detected_poses
self.action_result.blobImage = self.current_image_blob
self._action_server.set_succeeded(self.action_result)
else:
self.action_result.posesDetected = PoseArray()
self._action_server.set_succeeded(self.action_result)
def load_traj_msg(self, traj_x, traj_y):
if not len(traj_x) == len(traj_y):
raise ValueError("Trajectory not built correctly")
traj_pose_array = PoseArray(
) # Creation of a message with information on the chosen trajectory
traj_pose_array.header.stamp = rospy.Time.now()
traj_pose_array.header.frame_id = 'qualisys'
for i in range(len(traj_x)):
pose = Pose()
pose.position.x = traj_x[i]
pose.position.y = traj_y[i]
traj_pose_array.poses.append(pose)
return traj_pose_array
def tmp_publish(frame, camera_points):
global g_arena_points
output_message = PoseArray()
for i in xrange(np.size(camera_points, axis=0)):
p = Pose()
p.position.x = camera_points[i, 0]
p.position.y = camera_points[i, 1]
p.position.z = camera_points[i, 2]
output_message.poses.append(p)
for i in xrange(np.size(g_arena_points, axis=0)):
p = Pose()
p.position.x = g_arena_points[i, 0]
p.position.y = g_arena_points[i, 1]
p.position.z = g_arena_points[i, 2]
output_message.poses.append(p)
output_message.header = Header()
output_message.header.stamp = rospy.Time.now()
output_message.header.frame_id = frame
g_pub_dbg.publish(output_message)
def publish_particles(self, msg):
"""
Publishes particle poses on the map.
Uses Paul's default code at the moment, maybe later attempt to publish a visualization/MarkerArray
"""
particles_conv = []
for num, p in enumerate(self.particle_cloud):
particles_conv.append(p.as_pose())
self.particle_pub.publish(
PoseArray(header=Header(stamp=rospy.Time.now(),
frame_id=self.map_frame),
poses=particles_conv))
def _publish_places(self, places):
"""
使用PoseArray消息将位置发布为姿势
"""
if self._places_pub.get_num_connections() > 0:
msg = PoseArray()
msg.header.frame_id = self._robot.get_planning_frame()
msg.header.stamp = rospy.Time.now()
for place in places:
msg.poses.append(place.place_pose.pose)
self._places_pub.publish(msg)
print "-------------test14------------------"
def __init__(self):
# Create a publisher for acceleration data
self.pub = rospy.Publisher('/roomba/location_meters', PoseArray,
queue_size=10)
self.rate = rospy.Rate(10) # 10hz
# For finding radius
self.a = 391.3
self.b = -0.9371
self.radius = 0
# Altitude
self.alt = 0.8128 # 32 inches ~ as high as the countertop # NEEDS TO CHANGE FROM HARD CODE TO SUBSCRIPTION
#self.alt = 0.3048 # 12 inches ~ as high as the countertop
#self.alt = 1.143 # 45 inches ~ as high as the countertop
self.pose_array = PoseArray()
self.temp_pose = Pose()
self.header = Header()
self.header.seq = 0
self.header.stamp = rospy.Time.now()
self.cap = cv2.VideoCapture(0)
self.loop_search()
OUTPUT_FILE_VALUES = rospy.get_param('~output_file_values', 'view_values.json')
views = []
with open(INPUT_FILE, "r") as input_file:
json_data = input_file.read()
views = jsonpickle.decode(json_data)
rospy.loginfo("Loaded %s views" % len(views))
planner = ViewPlanner(robot)
view_values = planner.compute_view_values(views)
robot_poses = PoseArray()
robot_poses.header.frame_id = '/map'
robot_poses.poses = []
for v in views:
robot_poses.poses.append(v.get_ptu_pose())
robot_poses_pub.publish(robot_poses)
#view_costs = planner.compute_view_costs(views)
# triangle marker
markerArray = MarkerArray()
idx = 0
for view in views:
val = view_values[view.ID]
print idx, val
if val > 0:
def publish_pose(self,poses):
pose_array = PoseArray()
pose_array.header.stamp = rospy.Time.now()
pose_array.header.frame_id = 'map'
pose_array.poses = poses
self.particle_pub.publish(pose_array)
class PathPublisher():
# Must have __init__(self) function for a class, similar to a C++ class constructor.
def __init__(self):
# Get the ~private namespace parameters from command li
self.poseArray = PoseArray()
self.waypointArray = PoseArray()
self.filename = "/home/shart/marker_trajectories/hose/hose_waypoints_1.txt"
self.pathPub = rospy.Publisher("/path_publisher/path", Path)
self.posePub = rospy.Publisher("/path_publisher/poseArray", PoseArray)
self.waypointPub = rospy.Publisher("/path_publisher/waypointArray", PoseArray)
self.num_points = 10
self.frameOffset = Frame()
def readPoseArray(self) :
with open(self.filename) as f:
pa = f.read()
self.waypointArray.deserialize(pa)
[self.path, self.poseArray] = processPath(self.waypointArray, self.num_points)
print "path size: ", len(self.poseArray.poses)
self.waypointArray.header.frame_id = self.frame_id
self.poseArray.header.frame_id = self.frame_id
self.path.header.frame_id = self.frame_id
self.waypointArray.header.stamp = rospy.Time.now()
self.poseArray.header.frame_id = rospy.Time.now()
self.path.header.stamp = rospy.Time.now()
def publishPath(self) :
self.waypointArray.header.stamp = rospy.Time.now()
self.poseArray.header.frame_id = rospy.Time.now()
self.path.header.stamp = rospy.Time.now()
self.pathPub.publish(self.path)
self.posePub.publish(self.poseArray)
self.waypointPub.publish(self.waypointArray)
def setFile(self, fname) :
self.filename = fname
def setFrameID(self, fid) :
self.frame_id = fid
def setNumPoints(self, N) :
self.num_points = N
def applyFrameOffset(self, pose) :
self.frameOffset = getFrameFromPose(pose)
for i in range(len(self.poseArray.poses)) :
F = getFrameFromPose(self.poseArray.poses[i])
Fnew = self.frameOffset*F
self.poseArray.poses[i] = getPoseFromFrame(Fnew)
self.path.poses[i].pose = self.poseArray.poses[i]
for i in range(len(self.waypointArray.poses)) :
F = getFrameFromPose(self.waypointArray.poses[i])
Fnew = self.frameOffset*F
self.waypointArray.poses[i] = getPoseFromFrame(Fnew)
for i in range(len(self.path.poses)) :
self.path.poses[i].header.frame_id = self.frame_id
