Python sensor_msgs.msg.LaserScan() Examples

def get_observation_(self):
        """
        Function returns state that will be fed to the rl-agent
        It includes
        the raw laser scan data,
        the raw waypoint data.
        :return: state
        """
        waypoint = self.wp_
        num_of_wps = len(waypoint.points)
        state = np.ones(self.STATE_SIZE, dtype=np.float)

        # add laserscan
        state[0, 0:self.__scan_size, 0] = self.merged_scan_.ranges

        # add goal position
        wp_index = self.STATE_SIZE[1] - num_of_wps * 2
        for i in range(num_of_wps):
            state[0, (wp_index + i*2):(wp_index + i*2 + 2),0] = [waypoint.points[i].x, waypoint.points[i].y]

        # Discretize to a resolution of 5cm.
        state = np.round(np.divide(state, 0.05))*0.05
        if self.debug_:
            debug_scan = LaserScan()
            debug_scan.header = self.merged_scan_.header
            debug_scan.angle_min = self.merged_scan_.angle_min
            debug_scan.angle_max = self.merged_scan_.angle_max
            debug_scan.angle_increment = self.merged_scan_.angle_increment
            debug_scan.range_max = 7.0
            debug_scan.ranges = state[0, 0:self.__scan_size, 0]
            self.debugger_.show_input_scan(debug_scan)
        return 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 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 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 handle_turtle_pose(msg):
    br = tf.TransformBroadcaster()
    LaserScan laser_tf 

def get_observation_(self):
        """
        Function returns state that will be fed to the rl-agent
        It includes
        the raw laser scan data,
        the waypoint data in with the same format as the laser scan data.
        The distance of the waypoint is saved
        at the appropriate angle position in the vector.
        :return: state
        """
        waypoint = self.wp_
        num_of_wps = len(waypoint.points)

        state = np.ones(self.STATE_SIZE, dtype=np.float)

        # add laserscan
        state[ :, 0, 0] = self.merged_scan_.ranges

        # generate wp-vector
        wp_vector = np.zeros(self.STATE_SIZE[0])
        for i in range(num_of_wps):
            dist = math.sqrt(math.pow(waypoint.points[i].x, 2) + math.pow(waypoint.points[i].y, 2))
            angle = math.atan2(waypoint.points[i].y, waypoint.points[i].x) + math.pi
            wp_vector[math.floor(angle/self.merged_scan_.angle_increment)] = dist
        state[:,1,0] = wp_vector

        # Discretize to a resolution of 5cm.
        state = np.round(np.divide(state, self.__res))*self.__res
        if self.debug_:
            debug_scan = LaserScan()
            # debug_scan.header.frame_id = self.merged_scan_.header.frame_id
            debug_scan.header = self.merged_scan_.header
            debug_scan.angle_min = self.merged_scan_.angle_min
            debug_scan.angle_max = self.merged_scan_.angle_max
            debug_scan.angle_increment = self.merged_scan_.angle_increment
            debug_scan.range_max = 7.0
            debug_scan.ranges = state[:, 0, 0]
            self.debugger_.show_scan_stack(debug_scan)
        return state 

def __init__(self, ns, stack_offset):
        self.__ns = ns
        self.__stack_offset = stack_offset
        print("stack_offset: %d"%self.__stack_offset)
        self.__input_images = deque(maxlen=4 * self.__stack_offset)

        #Input state
        self.__input_img_pub1 = rospy.Publisher('%s/state_image1' % (self.__ns), Image, queue_size=1)
        self.__input_img_pub2 = rospy.Publisher('%s/state_image2' % (self.__ns), Image, queue_size=1)
        self.__input_img_pub3 = rospy.Publisher('%s/state_image3' % (self.__ns), Image, queue_size=1)
        self.__input_img_pub4 = rospy.Publisher('%s/state_image4' % (self.__ns), Image, queue_size=1)
        self.__occ_grid_pub = rospy.Publisher('%s/rl_map' % (self.__ns), OccupancyGrid, queue_size=1)
        self.__input_scan_pub = rospy.Publisher('%s/state_scan' % (self.__ns), LaserScan, queue_size=1)

        # reward info
        self.__rew_pub = rospy.Publisher('%s/reward' % (self.__ns), Marker, queue_size=1)
        self.__rew_num_pub = rospy.Publisher('%s/reward_num' % (self.__ns), Float64, queue_size=1)

        # Waypoint info
        self.__wp_pub1 = rospy.Publisher('%s/wp_vis1' % (self.__ns), PointStamped, queue_size=1)
        self.__wp_pub2 = rospy.Publisher('%s/wp_vis2' % (self.__ns), PointStamped, queue_size=1)
        self.__wp_pub3 = rospy.Publisher('%s/wp_vis3' % (self.__ns), PointStamped, queue_size=1)
        self.__wp_pub4 = rospy.Publisher('%s/wp_vis4' % (self.__ns), PointStamped, queue_size=1) 

def build_laser_scan(self, ranges):
        result = LaserScan()
        result.header.stamp = rospy.Time.now()
        result.angle_min = -almath.PI
        result.angle_max = almath.PI
        if len(ranges[1]) > 0:
            result.angle_increment = (result.angle_max - result.angle_min) / len(ranges[1])
        result.range_min = 0.0
        result.range_max = ranges[0]
        for range_it in ranges[1]:
            result.ranges.append(range_it[1])
        return result

    # do it! 

def _broadcastLasers(self, laser_publisher):
        """
        INTERNAL METHOD, publishes the laser values in the ROS framework

        Parameters:
            laser_publisher - The ROS publisher for the LaserScan message,
            corresponding to the laser info of the pepper robot (for API
            consistency)
        """
        if not self.robot.laser_manager.isActive():
            return

        scan = LaserScan()
        scan.header.stamp = rospy.get_rostime()
        scan.header.frame_id = "base_footprint"
        # -120 degres, 120 degres
        scan.angle_min = -2.0944
        scan.angle_max = 2.0944

        # 240 degres FoV, 61 points (blind zones inc)
        scan.angle_increment = (2 * 2.0944) / (15.0 + 15.0 + 15.0 + 8.0 + 8.0)

        # Detection ranges for the lasers in meters, 0.1 to 3.0 meters
        scan.range_min = 0.1
        scan.range_max = 3.0

        # Fill the lasers information
        right_scan = self.robot.getRightLaserValue()
        front_scan = self.robot.getFrontLaserValue()
        left_scan = self.robot.getLeftLaserValue()

        if isinstance(right_scan, list):
            scan.ranges.extend(list(reversed(right_scan)))
            scan.ranges.extend([-1]*8)
        if isinstance(front_scan, list):
            scan.ranges.extend(list(reversed(front_scan)))
            scan.ranges.extend([-1]*8)
        if isinstance(left_scan, list):
            scan.ranges.extend(list(reversed(left_scan)))

        laser_publisher.publish(scan) 

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 __init__(self):
		 # initiliaze
		rospy.init_node('GazeboWorld', anonymous=False)

		#-----------Default Robot State-----------------------
		self.set_self_state = ModelState()
		self.set_self_state.model_name = 'mobile_base' 
		self.set_self_state.pose.position.x = 0.5
		self.set_self_state.pose.position.y = 0.
		self.set_self_state.pose.position.z = 0.
		self.set_self_state.pose.orientation.x = 0.0
		self.set_self_state.pose.orientation.y = 0.0
		self.set_self_state.pose.orientation.z = 0.0
		self.set_self_state.pose.orientation.w = 1.0
		self.set_self_state.twist.linear.x = 0.
		self.set_self_state.twist.linear.y = 0.
		self.set_self_state.twist.linear.z = 0.
		self.set_self_state.twist.angular.x = 0.
		self.set_self_state.twist.angular.y = 0.
		self.set_self_state.twist.angular.z = 0.
		self.set_self_state.reference_frame = 'world'

		#------------Params--------------------
		self.depth_image_size = [160, 128]
		self.rgb_image_size = [304, 228]
		self.bridge = CvBridge()

		self.object_state = [0, 0, 0, 0]
		self.object_name = []

		# 0. | left 90/s | left 45/s | right 45/s | right 90/s | acc 1/s | slow down -1/s
		self.action_table = [0.4, 0.2, np.pi/6, np.pi/12, 0., -np.pi/12, -np.pi/6]
		
		self.self_speed = [.4, 0.0]
		self.default_states = None
		
		self.start_time = time.time()
		self.max_steps = 10000

		self.depth_image = None
		self.bump = False

		#-----------Publisher and Subscriber-------------
		self.cmd_vel = rospy.Publisher('cmd_vel_mux/input/navi', Twist, queue_size = 10)
		self.set_state = rospy.Publisher('gazebo/set_model_state', ModelState, queue_size = 10)
		self.resized_depth_img = rospy.Publisher('camera/depth/image_resized',Image, queue_size = 10)
		self.resized_rgb_img = rospy.Publisher('camera/rgb/image_resized',Image, queue_size = 10)

		self.object_state_sub = rospy.Subscriber('gazebo/model_states', ModelStates, self.ModelStateCallBack)
		self.depth_image_sub = rospy.Subscriber('camera/rgb/image_raw', Image, self.RGBImageCallBack)
		self.rgb_image_sub = rospy.Subscriber('camera/depth/image_raw', Image, self.DepthImageCallBack)
		self.laser_sub = rospy.Subscriber('scan', LaserScan, self.LaserScanCallBack)
		self.odom_sub = rospy.Subscriber('odom', Odometry, self.OdometryCallBack)
		self.bumper_sub = rospy.Subscriber('mobile_base/events/bumper', BumperEvent, self.BumperCallBack)

		rospy.sleep(2.)

		# What function to call when you ctrl + c    
		rospy.on_shutdown(self.shutdown) 

def _initPublishers(self):
        """
        INTERNAL METHOD, initializes the ROS publishers
        """
        self.front_cam_pub = rospy.Publisher(
            self.ros_namespace + '/camera/front/image_raw',
            Image,
            queue_size=10)

        self.front_info_pub = rospy.Publisher(
            self.ros_namespace + '/camera/front/camera_info',
            CameraInfo,
            queue_size=10)

        self.bottom_cam_pub = rospy.Publisher(
            self.ros_namespace + '/camera/bottom/image_raw',
            Image,
            queue_size=10)

        self.bottom_info_pub = rospy.Publisher(
            self.ros_namespace + '/camera/bottom/camera_info',
            CameraInfo,
            queue_size=10)

        self.depth_cam_pub = rospy.Publisher(
            self.ros_namespace + '/camera/depth/image_raw',
            Image,
            queue_size=10)

        self.depth_info_pub = rospy.Publisher(
            self.ros_namespace + '/camera/depth/camera_info',
            CameraInfo,
            queue_size=10)

        self.laser_pub = rospy.Publisher(
            self.ros_namespace + "/laser",
            LaserScan,
            queue_size=10)

        self.joint_states_pub = rospy.Publisher(
            '/joint_states',
            JointState,
            queue_size=10)

        self.odom_pub = rospy.Publisher(
            '/naoqi_driver/odom',
            Odometry,
            queue_size=10) 

def __init__(self, ns, train_mode):
        # Class variables
        self.__mode = train_mode                      # Mode of RL-agent (Training or Executrion ?)
        self.__ns = ns                          # namespace of simulation environment
        self.__is__new = False                  # True, if waypoint reached
        self.__static_scan = LaserScan()        # Laserscan only contains static objects
        self.__ped_scan = LaserScan()           # Laserscan only contains pedestrians
        self.__f_scan = LaserScan()
        self.__f_scan.header.frame_id = "base_footprint"
        self.__b_scan = LaserScan()
        self.__b_scan.header.frame_id = "base_footprint"

        self.__img = OccupancyGrid()            # input image
        self.__wps= Waypoint()                  # most recent Waypoints
        self.__twist = TwistStamped()           # most recent velocity
        self.__goal = Pose()                    # most recent goal position in robot frame
        self.__state_mode = 2                   # 0, if image as input state representation
                                                # 1, if stacked image representation in same frame
                                                # 2, if scan as input state representation



        # Subscriber
        self.wp_sub_ = rospy.Subscriber("%s/wp" % (self.__ns), Waypoint, self.__wp_callback, queue_size=1)

        if ["train", "eval"].__contains__(self.__mode):
            # Info only avaible during evaluation and training
            self.wp_sub_reached_ = rospy.Subscriber("%s/wp_reached" % (self.__ns), Waypoint, self.__wp_reached_callback, queue_size=1)

            self.static_scan_sub_ = rospy.Subscriber("%s/static_laser" % (self.__ns), LaserScan, self.__static_scan_callback,
                                                     queue_size=1)

            self.ped_scan_sub_ = rospy.Subscriber("%s/ped_laser" % (self.__ns), LaserScan, self.__ped_scan_callback,
                                                  queue_size=1)
            self.twist_sub_ = rospy.Subscriber("%s/twist" % (self.__ns), TwistStamped, self.__twist_callback, queue_size=1)
            self.goal_sub_ = rospy.Subscriber("%s/rl_agent/robot_to_goal" % (self.__ns), PoseStamped, self.__goal_callback, queue_size=1)
        else:
            self.static_scan_sub_ = rospy.Subscriber("%s/b_scan" % (self.__ns), LaserScan,
                                                     self.__b_scan_callback,
                                                     queue_size=1)
            self.static_scan_sub_ = rospy.Subscriber("%s/f_scan" % (self.__ns), LaserScan,
                                                     self.__f_scan_callback,
                                                     queue_size=1)

        # Service
        self.__img_srv = rospy.ServiceProxy('%s/image_generator/get_image' % (self.__ns), StateImageGenerationSrv)
        self.__sim_in_step = rospy.ServiceProxy('%s/is_in_step' % (self.__ns), SetBool)
        self.__merge_scans = rospy.ServiceProxy('%s/merge_scans' % (self.__ns), MergeScans)