我们从Python开源项目中，提取了以下26个代码示例，用于说明如何使用rospy.wait_for_message()。
def printJointStates(self): try: # now = rospy.Time.now() # self.tf.waitForTransform(self.frame, 'r_gripper_tool_frame', now, rospy.Duration(10.0)) self.tf.waitForTransform(self.frame, 'r_gripper_tool_frame', rospy.Time(), rospy.Duration(10.0)) currentRightPos, currentRightOrient = self.tf.lookupTransform(self.frame, 'r_gripper_tool_frame', rospy.Time(0)) msg = rospy.wait_for_message('/r_arm_controller/state', JointTrajectoryControllerState) currentRightJointPositions = msg.actual.positions print 'Right positions:', currentRightPos, currentRightOrient print 'Right joint positions:', currentRightJointPositions # now = rospy.Time.now() # self.tf.waitForTransform(self.frame, 'l_gripper_tool_frame', now, rospy.Duration(10.0)) currentLeftPos, currentLeftOrient = self.tf.lookupTransform(self.frame, 'l_gripper_tool_frame', rospy.Time(0)) msg = rospy.wait_for_message('/l_arm_controller/state', JointTrajectoryControllerState) currentLeftJointPositions = msg.actual.positions print 'Left positions:', currentLeftPos, currentLeftOrient print 'Left joint positions:', currentLeftJointPositions # print 'Right gripper state:', rospy.wait_for_message('/r_gripper_controller/state', JointControllerState) except tf.ExtrapolationException: print 'No transformation available! Failing to record this time step.'
def __init__(self): State.__init__(self, outcomes=['success'], input_keys=['waypoints'], output_keys=['waypoints']) # Create publsher to publish waypoints as pose array so that you can see them in rviz, etc. self.poseArray_publisher = rospy.Publisher('/waypoints', PoseArray, queue_size=1) # Start thread to listen for reset messages to clear the waypoint queue def wait_for_path_reset(): """thread worker function""" global waypoints while not rospy.is_shutdown(): data = rospy.wait_for_message('/path_reset', Empty) rospy.loginfo('Recieved path RESET message') self.initialize_path_queue() rospy.sleep(3) # Wait 3 seconds because `rostopic echo` latches # for three seconds and wait_for_message() in a # loop will see it again. reset_thread = threading.Thread(target=wait_for_path_reset) reset_thread.start()
def get_image_compressed(self): rospy.loginfo("Getting image...") image_msg = rospy.wait_for_message( "/wide_stereo/left/image_raw/compressed", CompressedImage) rospy.loginfo("Got image!") # Image to numpy array np_arr = np.fromstring(image_msg.data, np.uint8) # Decode to cv2 image and store cv2_img = cv2.imdecode(np_arr, cv2.CV_LOAD_IMAGE_COLOR) img_file_path = "/tmp/telegram_last_image.png" cv2.imwrite(img_file_path, cv2_img) rospy.loginfo("Saved to: " + img_file_path) return img_file_path # Define a few command handlers
def __init__(self, parent=None): super(ControlMainWindow, self).__init__(parent) self.ui = Ui_BaxterGUI() self.ui.setupUi(self) self.offline = True try: rospy.wait_for_message("/robot/state",AssemblyState,2.0) self.offline = False except rospy.exceptions.ROSException: self.offline = True self.general = TabGeneral(self) datapath = self.general.getPath("baxter_gui") + os.sep self.button = TabButton(self) if not self.offline: self.camera = TabCamera(self,datapath) self.infrared = TabInfrared(self) self.sonar = TabSonar(self) self.gripper = TabGripper(self) self.led = TabLed(self) self.display = TabDisplay(self) self.head = TabHead(self) self.arms = TabArms(self)
def grabImage(self,camera_name,filename): """ Grabs exactly one image from a camera :param camera_name: The image of the camera that should be saved :type camera_name: str :param filename: The full path of the filename where this image should be saved :type filename: str """ if self.open(camera_name) != 0: return msg=rospy.wait_for_message('/cameras/' + camera_name + "/image", Image) img = cv_bridge.CvBridge().imgmsg_to_cv2(msg, "bgr8") cv2.imwrite(filename,img) rospy.loginfo("Saved Image %s"%filename) self.close(camera_name)
def __init__(self): # Give the node a name rospy.init_node('odom_ekf', anonymous=False) # Publisher of type nav_msgs/Odometry self.ekf_pub = rospy.Publisher('output', Odometry, queue_size=5) # Wait for the /odom_combined topic to become available rospy.wait_for_message('input', PoseWithCovarianceStamped) # Subscribe to the /odom_combined topic rospy.Subscriber('input', PoseWithCovarianceStamped, self.pub_ekf_odom) rospy.loginfo("Publishing combined odometry on /odom_ekf")
def capture_sample(): """ Captures a PointCloud2 using the sensor stick RGBD camera Args: None Returns: PointCloud2: a single point cloud from the RGBD camrea """ get_model_state_prox = rospy.ServiceProxy('gazebo/get_model_state',GetModelState) model_state = get_model_state_prox('training_model','world') set_model_state_prox = rospy.ServiceProxy('gazebo/set_model_state', SetModelState) roll = random.uniform(0, 2*math.pi) pitch = random.uniform(0, 2*math.pi) yaw = random.uniform(0, 2*math.pi) quaternion = tf.transformations.quaternion_from_euler(roll, pitch, yaw) model_state.pose.orientation.x = quaternion[0] model_state.pose.orientation.y = quaternion[1] model_state.pose.orientation.z = quaternion[2] model_state.pose.orientation.w = quaternion[3] sms_req = SetModelStateRequest() sms_req.model_state.pose = model_state.pose sms_req.model_state.twist = model_state.twist sms_req.model_state.model_name = 'training_model' sms_req.model_state.reference_frame = 'world' set_model_state_prox(sms_req) return rospy.wait_for_message('/sensor_stick/point_cloud', PointCloud2)
def grabImage(self, sim=False, viz=False): # Grab image from Kinect sensor msg = rospy.wait_for_message('/semihaptics/image' if not sim else '/wide_stereo/left/image_color', Image) try: image = self.bridge.imgmsg_to_cv2(msg) image = cv2.cvtColor(image, cv2.COLOR_BGR2RGB) if viz: PILImage.fromarray(np.uint8(image)).show() return image except CvBridgeError, e: print e return None
def initJoints(self): msg = rospy.wait_for_message('/r_arm_controller/state', JointTrajectoryControllerState) self.rightJointNames = msg.joint_names self.initRightJointPositions = msg.actual.positions msg = rospy.wait_for_message('/l_arm_controller/state', JointTrajectoryControllerState) self.leftJointNames = msg.joint_names self.initLeftJointPositions = msg.actual.positions if self.verbose: print 'Right joint names:', self.rightJointNames print 'Left joint names:', self.leftJointNames
def grip(self, openGripper=True, maxEffort=200.0, rightArm=True, miniOpen=False, stopMovement=False): msg = Pr2GripperCommandGoal() if stopMovement: pos = rospy.wait_for_message('/r_gripper_controller/state', JointControllerState).process_value msg.command.position = pos else: msg.command.position = 0.1 if openGripper else (0.005 if miniOpen else 0.0) msg.command.max_effort = maxEffort if stopMovement or not openGripper else -1.0 if rightArm: self.rightGripperClient.send_goal(msg) else: self.leftGripperClient.send_goal(msg) # self.rightGripperClient.send_goal_and_wait(msg)
def get_topic_data(topic_name): """ GET /api/<version>/topic_data/<topic_name> Get a single data point from a topic over HTTP. """ topic_name = "/" + topic_name try: msg_class, real_topic, _ = rostopic.get_topic_class(topic_name) except rostopic.ROSTopicIOException as e: raise e if not real_topic: return error("Topic does not exist", 404) msg = rospy.wait_for_message(real_topic, msg_class) data = getattr(msg, "data", None) if data is None: json = '{"status": [' for x in msg.status: if x == msg.status[-1]: json += '{"name": \"%s\", "level": %d, "message": \"%s\", "hardware_id": \"%s\", "values": %s}]}' % \ (x.name if x.name else "null", x.level, \ x.message if x.message else "null", x.hardware_id if x.hardware_id else "null", \ x.values) else: json += '{"name": \"%s\", "level": %d, "message": \"%s\", "hardware_id": \"%s\", "values": %s},' % \ (x.name if x.name else "null", x.level, \ x.message if x.message else "null", x.hardware_id if x.hardware_id else "null", \ x.values) return Response(json, mimetype = 'application/json') else: return jsonify({"result": data})
def execute(self, userdata): global waypoints self.initialize_path_queue() self.path_ready = False # Start thread to listen for when the path is ready (this function will end then) def wait_for_path_ready(): """thread worker function""" data = rospy.wait_for_message('/path_ready', Empty) rospy.loginfo('Recieved path READY message') self.path_ready = True ready_thread = threading.Thread(target=wait_for_path_ready) ready_thread.start() topic = "/initialpose" rospy.loginfo("Waiting to recieve waypoints via Pose msg on topic %s" % topic) rospy.loginfo("To start following waypoints: 'rostopic pub /path_ready std_msgs/Empty -1'") # Wait for published waypoints while not self.path_ready: try: pose = rospy.wait_for_message(topic, PoseWithCovarianceStamped, timeout=1) except rospy.ROSException as e: if 'timeout exceeded' in e.message: continue # no new waypoint within timeout, looping... else: raise e rospy.loginfo("Recieved new waypoint") waypoints.append(pose) # publish waypoint queue as pose array so that you can see them in rviz, etc. self.poseArray_publisher.publish(convert_PoseWithCovArray_to_PoseArray(waypoints)) # Path is ready! return success and move on to the next state (FOLLOW_PATH) return 'success'
def getcurrentCartesianCommand(prefix_): # wait to get current position topic_address = '/j2n6s300_driver/out/cartesian_command' rospy.Subscriber(topic_address, kinova_msgs.msg.KinovaPose, setcurrentCartesianCommand) rospy.wait_for_message(topic_address, kinova_msgs.msg.KinovaPose) print 'position listener obtained message for Cartesian pose. '
def getCurrentFingerPosition(prefix_): # wait to get current position topic_address = '/' + prefix_ + 'driver/out/finger_position' rospy.Subscriber(topic_address, kinova_msgs.msg.FingerPosition, setCurrentFingerPosition) rospy.wait_for_message(topic_address, kinova_msgs.msg.FingerPosition) print 'obtained current finger position '
def getcurrentJointCommand(prefix_): # wait to get current position topic_address = '/j2n6s300_driver/out/joint_command' rospy.Subscriber(topic_address, kinova_msgs.msg.JointAngles, setcurrentJointCommand) rospy.wait_for_message(topic_address, kinova_msgs.msg.JointAngles) print 'position listener obtained message for joint position. '
def get_image(self, image_topic=None): rospy.loginfo("Getting image...") if image_topic is None: image_topic = "/wide_stereo/left/image_raw" image_msg = rospy.wait_for_message(image_topic, Image) rospy.loginfo("Got image!") cv2_img = self.bridge.imgmsg_to_cv2(image_msg, "bgr8") img_file_path = "/tmp/telegram_last_image.jpg" cv2.imwrite(img_file_path, cv2_img) rospy.loginfo("Saved to: " + img_file_path) return img_file_path # For some reason the image is grayscale...
def get_image(self): rospy.loginfo("Getting image...") image_msg = rospy.wait_for_message( "/wide_stereo/left/image_raw", Image) rospy.loginfo("Got image!") cv2_img = self.bridge.imgmsg_to_cv2(image_msg, "bgr8") img_file_path = "/tmp/telegram_last_image.jpg" cv2.imwrite(img_file_path, cv2_img) rospy.loginfo("Saved to: " + img_file_path) return img_file_path # For some reason the image is grayscale...
def __init__(self, joy_topic): rospy.loginfo("waiting for petrone") rospy.wait_for_message('battery', Float32) rospy.loginfo("found petrone") # fly control publisher self.pub_fly = rospy.Publisher('cmd_fly', Int8, queue_size=1) self.pub_led = rospy.Publisher('led_color', String, queue_size=1) # subscribe to the joystick at the end to make sure that all required # services were found self._buttons = None rospy.Subscriber(joy_topic, Joy, self.cb_joy)
def __init__(self): # initialize ROS node rospy.init_node('target_detector', anonymous=True) # initialize publisher for target pose, PoseStamped message, and set initial sequence number self.pub = rospy.Publisher('target_pose', PoseStamped, queue_size=1) self.pub_pose = PoseStamped() self.pub_pose.header.seq = 0 self.rate = rospy.Rate(1.0) # publish message at 1 Hz # initialize values for locating target on Kinect v2 image self.target_u = 0 # u is pixels left(0) to right(+) self.target_v = 0 # v is pixels top(0) to bottom(+) self.target_d = 0 # d is distance camera(0) to target(+) from depth image self.target_found = False # flag initialized to False self.last_d = 0 # last non-zero depth measurement # target orientation to Kinect v2 image (Euler) self.r = 0 self.p = 0 self.y = 0 # Convert image from a ROS image message to a CV image self.bridge = CvBridge() # Wait for the camera_info topic to become available rospy.wait_for_message('/kinect2/qhd/image_color_rect', Image) # Subscribe to registered color and depth images rospy.Subscriber('/kinect2/qhd/image_color_rect', Image, self.image_callback, queue_size=1) rospy.Subscriber('/kinect2/qhd/image_depth_rect', Image, self.depth_callback, queue_size=1) self.rate.sleep() # suspend until next cycle # This callback function handles processing Kinect color image, looking for the pink target.
def __init__(self): # initialize ROS node and transform publisher rospy.init_node('crazyflie_detector', anonymous=True) self.pub_tf = tf.TransformBroadcaster() self.rate = rospy.Rate(50.0) # publish transform at 50 Hz # initialize values for crazyflie location on Kinect v2 image self.cf_u = 0 # u is pixels left(0) to right(+) self.cf_v = 0 # v is pixels top(0) to bottom(+) self.cf_d = 0 # d is distance camera(0) to crazyflie(+) from depth image self.last_d = 0 # last non-zero depth measurement # crazyflie orientation to Kinect v2 image (Euler) self.r = -1.5708 self.p = 0 self.y = -3.1415 # Convert image from a ROS image message to a CV image self.bridge = CvBridge() cv2.namedWindow("KinectV2", 1) # Wait for the camera_info topic to become available rospy.wait_for_message('/kinect2/qhd/camera_info', CameraInfo) # Subscribe to Kinect v2 sd camera_info to get image frame height and width rospy.Subscriber('/kinect2/qhd/camera_info', CameraInfo, self.camera_data, queue_size=1) # Subscribe to registered color and depth images rospy.Subscriber('/kinect2/qhd/image_color_rect', Image, self.image_callback, queue_size=1) rospy.Subscriber('/kinect2/qhd/image_depth_rect', Image, self.depth_callback, queue_size=1) self.rate.sleep() # suspend until next cycle # This callback function sets parameters regarding the camera.
def __init__(self): rospy.init_node("follower") # Set the shutdown function (stop the robot) rospy.on_shutdown(self.shutdown) # The dimensions (in meters) of the box in which we will search # for the person (blob). These are given in camera coordinates # where x is left/right,y is up/down and z is depth (forward/backward) self.min_x = rospy.get_param("~min_x", -0.2) self.max_x = rospy.get_param("~max_x", 0.2) self.min_y = rospy.get_param("~min_y", -0.3) self.max_y = rospy.get_param("~max_y", 0.5) self.max_z = rospy.get_param("~max_z", 1.2) # The goal distance (in meters) to keep between the robot and the person self.goal_z = rospy.get_param("~goal_z", 0.6) # How far away from the goal distance (in meters) before the robot reacts self.z_threshold = rospy.get_param("~z_threshold", 0.05) # How far away from being centered (x displacement) on the person # before the robot reacts self.x_threshold = rospy.get_param("~x_threshold", 0.05) # How much do we weight the goal distance (z) when making a movement self.z_scale = rospy.get_param("~z_scale", 1.0) # How much do we weight left/right displacement of the person when making a movement self.x_scale = rospy.get_param("~x_scale", 2.5) # The maximum rotation speed in radians per second self.max_angular_speed = rospy.get_param("~max_angular_speed", 2.0) # The minimum rotation speed in radians per second self.min_angular_speed = rospy.get_param("~min_angular_speed", 0.0) # The max linear speed in meters per second self.max_linear_speed = rospy.get_param("~max_linear_speed", 0.3) # The minimum linear speed in meters per second self.min_linear_speed = rospy.get_param("~min_linear_speed", 0.1) # Slow down factor when stopping self.slow_down_factor = rospy.get_param("~slow_down_factor", 0.8) # Initialize the movement command self.move_cmd = Twist() # Publisher to control the robot's movement self.cmd_vel_pub = rospy.Publisher('cmd_vel', Twist, queue_size=5) # Subscribe to the point cloud self.depth_subscriber = rospy.Subscriber('point_cloud', PointCloud2, self.set_cmd_vel, queue_size=1) rospy.loginfo("Subscribing to point cloud...") # Wait for the pointcloud topic to become available rospy.wait_for_message('point_cloud', PointCloud2) rospy.loginfo("Ready to follow!")
def __init__(self): rospy.init_node("follower") # Set the shutdown function (stop the robot) rospy.on_shutdown(self.shutdown) # The goal distance (in meters) to keep between the robot and the person self.goal_z = rospy.get_param("~goal_z", 0.6) # How far away from the goal distance (in meters) before the robot reacts self.z_threshold = rospy.get_param("~z_threshold", 0.05) # How far away from being centered (x displacement) on the person # before the robot reacts self.x_threshold = rospy.get_param("~x_threshold", 0.05) # How much do we weight the goal distance (z) when making a movement self.z_scale = rospy.get_param("~z_scale", 1.0) # How much do we weight x-displacement of the person when making a movement self.x_scale = rospy.get_param("~x_scale", 2.5) # The maximum rotation speed in radians per second self.max_angular_speed = rospy.get_param("~max_angular_speed", 2.0) # The minimum rotation speed in radians per second self.min_angular_speed = rospy.get_param("~min_angular_speed", 0.0) # The max linear speed in meters per second self.max_linear_speed = rospy.get_param("~max_linear_speed", 0.3) # The minimum linear speed in meters per second self.min_linear_speed = rospy.get_param("~min_linear_speed", 0.1) # Slow down factor when stopping self.slow_down_factor = rospy.get_param("~slow_down_factor", 0.8) # Initialize the movement command self.move_cmd = Twist() # Publisher to control the robot's movement self.cmd_vel_pub = rospy.Publisher('/cmd_vel', Twist, queue_size=5) # Subscribe to the point cloud self.depth_subscriber = rospy.Subscriber('point_cloud', PointCloud2, self.set_cmd_vel, queue_size=1) rospy.loginfo("Subscribing to point cloud...") # Wait for the pointcloud topic to become available rospy.wait_for_message('point_cloud', PointCloud2) rospy.loginfo("Ready to follow!")
