Python rospy.wait_for_message() Examples

def run(self):
        while not rospy.is_shutdown():
            msg = CNN_out()
            msg.header.stamp = rospy.Time.now()
            data = None
            while data is None:
                try:
                    data = rospy.wait_for_message("camera", Image, timeout=10)
                except:
                    pass

            if self.use_network_out:
                print("Publishing commands!")
            else:
                print("NOT Publishing commands!")

            cv_image = utils.callback_img(data, self.target_size, self.crop_size,
                self.imgs_rootpath, self.use_network_out)
            outs = self.model.predict_on_batch(cv_image[None])
            steer, coll = outs[0][0], outs[1][0]
            msg.steering_angle = steer
            msg.collision_prob = coll
            self.pub.publish(msg) 

def get_current_fk(self, fk_link_names, frame_id='base_link'):
        """
        Get the current forward kinematics of a set of links.

        :param fk_link_names: [string]
                        list of links that we want to
                        get the forward kinematics from
        :param frame_id: string
                        the reference frame to be used
        :return fk_result: moveit_msgs.srv.GetPositionFKResponse
        """
        # Subscribe to a joint_states
        js = rospy.wait_for_message('/robot/joint_states', JointState)
        # Call FK service
        fk_result = self.get_fk(fk_link_names, js.name, js.position, frame_id)
        return fk_result 

def start(self):
        """ Start the sensor """
        # initialize subscribers
        self._image_sub = rospy.Subscriber(self.topic_image_color, sensor_msgs.msg.Image, self._color_image_callback)
        self._depth_sub = rospy.Subscriber(self.topic_image_depth, sensor_msgs.msg.Image, self._depth_image_callback)
        self._camera_info_sub = rospy.Subscriber(self.topic_info_camera, sensor_msgs.msg.CameraInfo, self._camera_info_callback)
        
        timeout = 10
        try:
            rospy.loginfo("waiting to recieve a message from the Kinect")
            rospy.wait_for_message(self.topic_image_color, sensor_msgs.msg.Image, timeout=timeout)
            rospy.wait_for_message(self.topic_image_depth, sensor_msgs.msg.Image, timeout=timeout)
            rospy.wait_for_message(self.topic_info_camera, sensor_msgs.msg.CameraInfo, timeout=timeout)
        except rospy.ROSException as e:
            print("KINECT NOT FOUND")
            rospy.logerr("Kinect topic not found, Kinect not started")
            rospy.logerr(e)

        while self._camera_intr is None:
            time.sleep(0.1)
        
        self._running = True 

def test_vehicle_info(self):
        """
        Tests vehicle_info
        """
        rospy.init_node('test_node', anonymous=True)
        msg = rospy.wait_for_message(
            "/carla/ego_vehicle/vehicle_info", CarlaEgoVehicleInfo, timeout=TIMEOUT)
        self.assertNotEqual(msg.id, 0)
        self.assertEqual(msg.type, "vehicle.tesla.model3")
        self.assertEqual(msg.rolename, "ego_vehicle")
        self.assertEqual(len(msg.wheels), 4)
        self.assertNotEqual(msg.max_rpm, 0.0)
        self.assertNotEqual(msg.moi, 0.0)
        self.assertNotEqual(msg.damping_rate_full_throttle, 0.0)
        self.assertNotEqual(msg.damping_rate_zero_throttle_clutch_engaged, 0.0)
        self.assertNotEqual(
            msg.damping_rate_zero_throttle_clutch_disengaged, 0.0)
        self.assertTrue(msg.use_gear_autobox)
        self.assertNotEqual(msg.gear_switch_time, 0.0)
        self.assertNotEqual(msg.mass, 0.0)
        self.assertNotEqual(msg.clutch_strength, 0.0)
        self.assertNotEqual(msg.drag_coefficient, 0.0)
        self.assertNotEqual(msg.center_of_mass, Vector3()) 

def reset(self):
        rospy.wait_for_service('gazebo/reset_simulation')
        try:
            self.reset_proxy()
        except (rospy.ServiceException) as e:
            print("gazebo/reset_simulation service call failed")

        data = None
        while data is None:
            try:
                data = rospy.wait_for_message('scan', LaserScan, timeout=5)
            except:
                pass

        if self.initGoal:
            self.goal_x, self.goal_y = self.respawn_goal.getPosition()
            self.initGoal = False

        self.goal_distance = self.getGoalDistace()
        state, done = self.getState(data)

        return np.asarray(state) 

def run(self):
        """
        main loop
        """
        # wait for ros-bridge to set up CARLA world
        rospy.loginfo("Waiting for CARLA world (topic: /carla/world_info)...")
        try:
            rospy.wait_for_message("/carla/world_info", CarlaWorldInfo, timeout=10.0)
        except rospy.ROSException as e:
            rospy.logerr("Timeout while waiting for world info!")
            raise e
        rospy.loginfo("CARLA world available. Spawn infrastructure...")
        client = carla.Client(self.host, self.port)
        client.set_timeout(self.timeout)
        self.world = client.get_world()
        self.restart()
        try:
            rospy.spin()
        except rospy.ROSInterruptException:
            pass

# ==============================================================================
# -- main() --------------------------------------------------------------------
# ============================================================================== 

def step(self, action):
        max_angular_vel = 1.5
        ang_vel = ((self.action_size - 1)/2 - action) * max_angular_vel * 0.5

        vel_cmd = Twist()
        vel_cmd.linear.x = 0.15
        vel_cmd.angular.z = ang_vel
        self.pub_cmd_vel.publish(vel_cmd)

        data = None
        while data is None:
            try:
                data = rospy.wait_for_message('scan', LaserScan, timeout=5)
            except:
                pass

        state, done = self.getState(data)
        reward = self.setReward(state, done, action)

        return np.asarray(state), reward, done 

def __init__(self):
        # Get the camera parameters
        cam_info_topic = rospy.get_param('~camera/info_topic')
        camera_info_msg = rospy.wait_for_message(cam_info_topic, CameraInfo)
        self.cam_K = np.array(camera_info_msg.K).reshape((3, 3))

        self.img_pub = rospy.Publisher('~visualisation', Image, queue_size=1)
        rospy.Service('~predict', GraspPrediction, self.compute_service_handler)

        self.base_frame = rospy.get_param('~camera/robot_base_frame')
        self.camera_frame = rospy.get_param('~camera/camera_frame')
        self.img_crop_size = rospy.get_param('~camera/crop_size')
        self.img_crop_y_offset = rospy.get_param('~camera/crop_y_offset')
        self.cam_fov = rospy.get_param('~camera/fov')

        self.counter = 0
        self.curr_depth_img = None
        self.curr_img_time = 0
        self.last_image_pose = None
        rospy.Subscriber(rospy.get_param('~camera/depth_topic'), Image, self._depth_img_callback, queue_size=1)

        self.waiting = False
        self.received = False 

def step(self, action):
        max_angular_vel = 1.5
        ang_vel = ((self.action_size - 1)/2 - action) * max_angular_vel * 0.5

        vel_cmd = Twist()
        vel_cmd.linear.x = 0.15
        vel_cmd.angular.z = ang_vel
        self.pub_cmd_vel.publish(vel_cmd)

        data = None
        while data is None:
            try:
                data = rospy.wait_for_message('scan', LaserScan, timeout=5)
            except:
                pass

        state, done = self.getState(data)
        reward = self.setReward(state, done, action)

        return np.asarray(state), reward, done 

def reset(self):
        rospy.wait_for_service('gazebo/reset_simulation')
        try:
            self.reset_proxy()
        except (rospy.ServiceException) as e:
            print("gazebo/reset_simulation service call failed")

        data = None
        while data is None:
            try:
                data = rospy.wait_for_message('scan', LaserScan, timeout=5)
            except:
                pass

        if self.initGoal:
            self.goal_x, self.goal_y = self.respawn_goal.getPosition()
            self.initGoal = False

        self.goal_distance = self.getGoalDistace()
        state, done = self.getState(data)

        return np.asarray(state) 

def step(self, action):
        max_angular_vel = 1.5
        ang_vel = ((self.action_size - 1)/2 - action) * max_angular_vel * 0.5

        vel_cmd = Twist()
        vel_cmd.linear.x = 0.15
        vel_cmd.angular.z = ang_vel
        self.pub_cmd_vel.publish(vel_cmd)

        data = None
        while data is None:
            try:
                data = rospy.wait_for_message('scan', LaserScan, timeout=5)
            except:
                pass

        state, done = self.getState(data)
        reward = self.setReward(state, done, action)

        return np.asarray(state), reward, done 

def step(self, action):
        max_angular_vel = 1.5
        ang_vel = ((self.action_size - 1)/2 - action) * max_angular_vel * 0.5

        vel_cmd = Twist()
        vel_cmd.linear.x = 0.15
        vel_cmd.angular.z = ang_vel
        self.pub_cmd_vel.publish(vel_cmd)

        data = None
        while data is None:
            try:
                data = rospy.wait_for_message('scan', LaserScan, timeout=5)
            except:
                pass

        state, done = self.getState(data)
        reward = self.setReward(state, done, action)

        return np.asarray(state), reward, done 

def main():
    rospy.init_node('jaco_jtas_test')
    dof = rospy.get_param('~jaco_dof')
    
    tmp = rospy.wait_for_message("/movo/right_arm/joint_states", JointState)
    current_angles= tmp.position
    traj = JacoJTASTest('right')
    traj.add_point(current_angles, 0.0)

    if '6dof' == dof:
        p1 = [0.0] * 6
    if '7dof' == dof:
        p1 = [0.0] * 7

    traj.add_point(p1,10.0)
    p2 = list(current_angles)
    traj.add_point(p2,20.0)
    traj.start()

    traj.wait(20.0)
    print("Exiting - Joint Trajectory Action Test Complete") 

def __init__(self, camera_name, base_frame, table_height):
        """
        Initialize the instance

        :param camera_name: The camera name. One of (head_camera, right_hand_camera)
        :param base_frame: The frame for the robot base
        :param table_height: The table height with respect to base_frame
        """
        self.camera_name = camera_name
        self.base_frame = base_frame
        self.table_height = table_height

        self.image_queue = Queue.Queue()
        self.pinhole_camera_model = PinholeCameraModel()
        self.tf_listener = tf.TransformListener()

        camera_info_topic = "/io/internal_camera/{}/camera_info".format(camera_name)
        camera_info = rospy.wait_for_message(camera_info_topic, CameraInfo)

        self.pinhole_camera_model.fromCameraInfo(camera_info)

        cameras = intera_interface.Cameras()
        cameras.set_callback(camera_name, self.__show_image_callback, rectify_image=True) 

def test_camera_image(self):
        """
        Tests camera_images
        """
        rospy.init_node('test_node', anonymous=True)
        msg = rospy.wait_for_message(
            "/carla/ego_vehicle/camera/rgb/front/image_color", Image, timeout=TIMEOUT)
        self.assertEqual(msg.header.frame_id, "ego_vehicle/camera/rgb/front")
        self.assertEqual(msg.height, 600)
        self.assertEqual(msg.width, 800)
        self.assertEqual(msg.encoding, "bgra8") 

def test_lidar(self):
        """
        Tests Lidar sensor node
        """
        rospy.init_node('test_node', anonymous=True)
        msg = rospy.wait_for_message(
            "/carla/ego_vehicle/lidar/lidar1/point_cloud", PointCloud2, timeout=TIMEOUT)
        self.assertEqual(msg.header.frame_id, "ego_vehicle/lidar/lidar1") 

def test_radar(self):
        """
        Tests Radar sensor node
        """
        rospy.init_node('test_node', anonymous=True)
        msg = rospy.wait_for_message(
            "/carla/ego_vehicle/radar/front/radar", CarlaRadarMeasurement, timeout=TIMEOUT)
        self.assertEqual(msg.header.frame_id, "ego_vehicle/radar/front") 

def test_ego_vehicle_objects(self):
        """
        Tests objects node for ego_vehicle
        """
        rospy.init_node('test_node', anonymous=True)
        msg = rospy.wait_for_message(
            "/carla/ego_vehicle/objects", ObjectArray, timeout=15)
        self.assertEqual(msg.header.frame_id, "map")
        self.assertEqual(len(msg.objects), 0) 

def test_objects(self):
        """
        Tests carla objects
        """
        rospy.init_node('test_node', anonymous=True)
        msg = rospy.wait_for_message("/carla/objects", ObjectArray, timeout=TIMEOUT)
        self.assertEqual(msg.header.frame_id, "map")
        self.assertEqual(len(msg.objects), 1)  # only ego vehicle exists 

def main():
    rospy.init_node('torso_jtas_test')
    
    tmp = rospy.wait_for_message("/movo/linear_actuator/joint_states", JointState)
    current_angles= tmp.position
    print current_angles
    traj = TorsoJTASTest()
    traj.add_point(list(current_angles), 0.0)
    
    total_time = 0.0
    points = [list(current_angles), 0.0]
    for i in range(0,10):
        
        pos = random.uniform(0.0,0.4)
        vel = random.uniform(0.02,0.06)
        
        
        
        dt = abs(pos)/vel
        total_time+=dt
   
        traj.add_point([pos],total_time)
        
        
    traj.start()

    traj.wait(total_time+3.0)
    print("Exiting - Joint Trajectory Action Test Complete") 

def test_world_info(self):
        """
        Tests world_info
        """
        rospy.init_node('test_node', anonymous=True)
        msg = rospy.wait_for_message(
            "/carla/world_info", CarlaWorldInfo, timeout=TIMEOUT)
        self.assertNotEqual(len(msg.map_name), 0)
        self.assertNotEqual(len(msg.opendrive), 0) 

def test_actor_list(self):
        """
        Tests actor_list
        """
        rospy.init_node('test_node', anonymous=True)
        msg = rospy.wait_for_message(
            "/carla/actor_list", CarlaActorList, timeout=TIMEOUT)
        self.assertNotEqual(len(msg.actors), 0) 

def test_traffic_lights(self):
        """
        Tests traffic_lights
        """
        rospy.init_node('test_node', anonymous=True)
        msg = rospy.wait_for_message(
            "/carla/traffic_lights", CarlaTrafficLightStatusList, timeout=TIMEOUT)
        self.assertNotEqual(len(msg.traffic_lights), 0) 

def test_traffic_lights_info(self):
        """
        Tests traffic_lights
        """
        rospy.init_node('test_node', anonymous=True)
        msg = rospy.wait_for_message(
            "/carla/traffic_lights_info", CarlaTrafficLightInfoList, timeout=TIMEOUT)
        self.assertNotEqual(len(msg.traffic_lights), 0) 

def run(self):
        """
        main loop
        """
        # wait for ros-bridge to set up CARLA world
        rospy.loginfo("Waiting for CARLA world (topic: /carla/world_info)...")
        try:
            rospy.wait_for_message("/carla/world_info", CarlaWorldInfo, timeout=10.0)
        except rospy.ROSException:
            rospy.logerr("Timeout while waiting for world info!")
            sys.exit(1)

        rospy.loginfo("CARLA world available. Spawn ego vehicle...")

        client = carla.Client(self.host, self.port)
        client.set_timeout(self.timeout)
        self.world = client.get_world()
        self.restart()
        try:
            rospy.spin()
        except rospy.ROSInterruptException:
            pass

# ==============================================================================
# -- main() --------------------------------------------------------------------
# ============================================================================== 

def reset(self):
        rospy.wait_for_service('gazebo/reset_simulation')
        try:
            self.reset_proxy()
        except (rospy.ServiceException) as e:
            print("gazebo/reset_simulation service call failed")

        data = None
        while data is None:
            try:
                data = rospy.wait_for_message('scan', LaserScan, timeout=5)
            except:
                pass

        if self.initGoal:
            self.goal_x, self.goal_y = self.respawn_goal.getPosition()
            self.initGoal = False

        self.goal_distance = self.getGoalDistace()
        state, done = self.getState(data)

        return np.asarray(state) 

def main():
    rospy.init_node('pan_tilt_jtas_test')
    
    tmp = rospy.wait_for_message("/movo/head/joint_states", JointState)
    current_angles= tmp.position
    print current_angles
    traj = HeadJTASTest()
    traj.add_point(list(current_angles), 0.0)
    
    total_time = 0.0
    points = [list(current_angles), 0.0]
    for i in range(0,10):
        
        pos = [random.uniform(-1.57,1.57),random.uniform(-1.57,1.57)]
        vel = random.uniform(0.2,0.5)
        
        dt = 0.0
        for i in range(2):
            tmp = abs(pos[i])/vel
            if (tmp > dt):
                dt = tmp
        total_time+=dt
   
        traj.add_point(pos,total_time)
        
        
    traj.start()

    traj.wait(total_time+3.0)
    print("Exiting - Joint Trajectory Action Test Complete") 

def step(self, action):
		# Publish action
		if action==0:
			self.left_pub.publish(self.v_forward-self.v_turn)
			self.right_pub.publish(self.v_forward+self.v_turn)
			self.rate.sleep()
		elif action==1:
			self.left_pub.publish(self.v_forward)
			self.right_pub.publish(self.v_forward)
			self.rate.sleep()
		elif action==2:
			self.left_pub.publish(self.v_forward+self.v_turn)
			self.right_pub.publish(self.v_forward-self.v_turn)
			self.rate.sleep()
		# Get transform data
		p = rospy.wait_for_message('transformData', Transform).translation
		p = np.array([p.x,p.y])
		# Calculate robot position and distance
		d, p = self.getDistance(p)
		# Calculate reward
		r = normpdf(d)
		# Translate DVS data from FIFO queue into state image
		s = self.getState()
		# Check if distance causes reset
		if abs(d) > self.reset_distance:
			return s, r, True, d, p
		else:
			return s, r, False, d, p 

def test_gnss(self):
        """
        Tests Gnss
        """
        rospy.init_node('test_node', anonymous=True)
        msg = rospy.wait_for_message(
            "/carla/ego_vehicle/gnss/gnss1/fix", NavSatFix, timeout=TIMEOUT)
        self.assertEqual(msg.header.frame_id, "ego_vehicle/gnss/gnss1")
        self.assertNotEqual(msg.latitude, 0.0)
        self.assertNotEqual(msg.longitude, 0.0)
        self.assertNotEqual(msg.altitude, 0.0) 

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")