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