Python msg.OccupancyGrid方法代码示例

# 需要导入模块: from nav_msgs import msg [as 别名]
# 或者: from nav_msgs.msg import OccupancyGrid [as 别名]
def __init__(self, image_path=None):
        height = int(rospy.get_param("~grid_height", 800))
        width = int(rospy.get_param("~grid_width", 800))
        resolution = rospy.get_param("~grid_resolution", .25)
        ogrid_topic = rospy.get_param("/lqrrt_node/ogrid_topic", "/ogrid")

        self.grid_drawer = DrawGrid(height, width, image_path)
        self.ogrid_pub = rospy.Publisher(ogrid_topic, OccupancyGrid, queue_size=1)

        m = MapMetaData()
        m.resolution = resolution
        m.width = width
        m.height = height
        pos = np.array([-width * resolution / 2, -height * resolution / 2, 0])
        quat = np.array([0, 0, 0, 1])
        m.origin = Pose()
        m.origin.position.x, m.origin.position.y = pos[:2]
        self.map_meta_data = m

        rospy.Timer(rospy.Duration(1), self.pub_grid) 

# 需要导入模块: from nav_msgs import msg [as 别名]
# 或者: from nav_msgs.msg import OccupancyGrid [as 别名]
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) 

# 需要导入模块: from nav_msgs import msg [as 别名]
# 或者: from nav_msgs.msg import OccupancyGrid [as 别名]
def ogrid_cb(self, msg):
        """
        Expects an OccupancyGrid message.
        Stores the ogrid array and origin vector.
        Reevaluates the current plan since the ogrid changed.

        """
        start = self.rostime()
        self.ogrid = np.array(msg.data).reshape((msg.info.height, msg.info.width))
        self.ogrid_origin = np.array([msg.info.origin.position.x, msg.info.origin.position.y])
        self.ogrid_cpm = 1 / msg.info.resolution
        try:
            self.reevaluate_plan()
        except:
            print("\n(WARNING: something went wrong in reevaluate_plan)\n")
        elapsed = abs(self.rostime() - start)
        if elapsed > 1:
            print("\n(WARNING: ogrid callback is taking {} seconds)\n".format(np.round(elapsed, 2))) 

# 需要导入模块: from nav_msgs import msg [as 别名]
# 或者: from nav_msgs.msg import OccupancyGrid [as 别名]
def __init__(self):
        self.map = None
        self.start = None
        self.goal = None

        self.moves = [Move(0.1, 0),  # forward
                      Move(-0.1, 0),  # back
                      Move(0, 1.5708),  # turn left 90
                      Move(0, -1.5708)] # turn right 90
        self.robot = Robot(0.5, 0.5)
        self.is_working = False # Replace with mutex after all

        self.map_subscriber = rospy.Subscriber("map", OccupancyGrid, self.new_map_callback)
        self.start_subscriber = rospy.Subscriber("initialpose", PoseWithCovarianceStamped, self.new_start_callback)
        self.goal_subscriber = rospy.Subscriber("goal", PoseStamped, self.new_goal_callback)

        self.path_publisher = rospy.Publisher("trajectory", MarkerArray, queue_size=1)
        self.pose_publisher = rospy.Publisher("debug_pose", PoseStamped, queue_size=1)

        # what will be there. A module goes into variable. Isn't it too much memory consumption. Maybe I should assign function replan() to this variable?
        self.planner = planners.astar.replan 

# 需要导入模块: from nav_msgs import msg [as 别名]
# 或者: from nav_msgs.msg import OccupancyGrid [as 别名]
def numpy_to_occupancy_grid(arr, info=None):
	if not len(arr.shape) == 2:
		raise TypeError('Array must be 2D')
	if not arr.dtype == np.int8:
		raise TypeError('Array must be of int8s')

	grid = OccupancyGrid()
	if isinstance(arr, np.ma.MaskedArray):
		# We assume that the masked value are already -1, for speed
		arr = arr.data
	grid.data = arr.ravel()
	grid.info = info or MapMetaData()
	grid.info.height = arr.shape[0]
	grid.info.width = arr.shape[1]

	return grid 

# 需要导入模块: from nav_msgs import msg [as 别名]
# 或者: from nav_msgs.msg import OccupancyGrid [as 别名]
def test_serialization(self):
        msg = OccupancyGrid(
            info=MapMetaData(
                width=3,
                height=3
            ),
            data = [0, 0, 0, 0, -1, 0, 0, 0, 0]
        )

        data = ros_numpy.numpify(msg)
        self.assertIs(data[1,1], np.ma.masked)
        msg2 = ros_numpy.msgify(OccupancyGrid, data)

        self.assertEqual(msg.info, msg2.info)

        io1 = BytesIO()
        io2 = BytesIO()
        msg.serialize(io1)
        msg2.serialize(io2)

        self.assertEqual(io1.getvalue(), io2.getvalue(), "Message serialization survives round-trip") 

# 需要导入模块: from nav_msgs import msg [as 别名]
# 或者: from nav_msgs.msg import OccupancyGrid [as 别名]
def pub_grid(self, *args):
        grid = self.grid_drawer.img

        ogrid = OccupancyGrid()
        ogrid.header.frame_id = '/world'
        ogrid.info = self.map_meta_data
        ogrid.data = np.subtract(np.flipud(grid).flatten(), 1).astype(np.int8).tolist()

        self.ogrid_pub.publish(ogrid) 

# 需要导入模块: from nav_msgs import msg [as 别名]
# 或者: from nav_msgs.msg import OccupancyGrid [as 别名]
def _init_occupancy_map(self):
        grid = OccupancyGrid()
        grid.header.seq = 1
        grid.header.frame_id = "/map"
        grid.info.origin.position.z = 0
        grid.info.origin.orientation.x = 0
        grid.info.origin.orientation.y = 0
        grid.info.origin.orientation.z = 0
        grid.info.origin.orientation.w = 1
        return grid 

# 需要导入模块: from nav_msgs import msg [as 别名]
# 或者: from nav_msgs.msg import OccupancyGrid [as 别名]
def __init__(self, z_lower_treshold, z_upper_treshold, x_min, y_min, x_max, y_max):

        self.bot = Robot(
            "locobot", base_config={"build_map": True, "base_planner": "none"}
        )

        self.ocGrid = None  #
        self.points = None  # these are the points from orbslam
        self.colors = None

        self.z_lower_treshold = z_lower_treshold
        self.z_upper_treshold = z_upper_treshold

        # occupancy grid params
        self.xMax = None
        self.xMin = None
        self.yMin = None
        self.yMax = None

        # grid
        self.grid = OccupancyGrid()
        self.xCells = None
        self.yCells = None

        self.x_min = x_min
        self.x_max = x_max
        self.y_min = y_min
        self.y_max = y_max

        self.initGrid() 

# 需要导入模块: from nav_msgs import msg [as 别名]
# 或者: from nav_msgs.msg import OccupancyGrid [as 别名]
def main():
    convertor = PcdToOcGrid(0.3, 0.5, -2.5, -2.5, 2.5, 2.5)
    ocGrid_publisher = rospy.Publisher(MAP_TOPIC, OccupancyGrid, queue_size=1)
    rate = rospy.Rate(10)  # 10hz

    while not rospy.is_shutdown():
        convertor.populatePoints()
        convertor.calcSize()
        convertor.populateMap()
        convertor.updateGrid()
        ocGrid_publisher.publish(convertor.grid)
        rate.sleep() 

# 需要导入模块: from nav_msgs import msg [as 别名]
# 或者: from nav_msgs.msg import OccupancyGrid [as 别名]
def __init__(self):
        NaoqiNode.__init__(self, 'naoqi_localization')
        self.navigation = None
        self.connectNaoQi()
        self.publishers = {}
        self.publishers["uncertainty"] = rospy.Publisher('localization_uncertainty', Marker)
        self.publishers["map_tf"] = rospy.Publisher('/tf', TFMessage, latch=True)
        self.publishers["map"] = rospy.Publisher('naoqi_exploration_map', OccupancyGrid , queue_size=None)
        self.publishers["exploration_path"] = rospy.Publisher('naoqi_exploration_path', Path, queue_size=None)
        self.frequency = 2
        self.rate = rospy.Rate(self.frequency)

    # (re-) connect to NaoQI: 

# 需要导入模块: from nav_msgs import msg [as 别名]
# 或者: from nav_msgs.msg import OccupancyGrid [as 别名]
def test_masking(self):
        data = -np.ones((30, 30), np.int8)
        data[10:20, 10:20] = 100

        msg = ros_numpy.msgify(OccupancyGrid, data)

        data_out = ros_numpy.numpify(msg)

        self.assertIs(data_out[5, 5], np.ma.masked)
        np.testing.assert_equal(data_out[10:20, 10:20], 100) 

# 需要导入模块: from nav_msgs import msg [as 别名]
# 或者: from nav_msgs.msg import OccupancyGrid [as 别名]
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) 

# 需要导入模块: from nav_msgs import msg [as 别名]
# 或者: from nav_msgs.msg import OccupancyGrid [as 别名]
def __init__(self, odom_topic, ref_topic, move_topic, path_topic, tree_topic,
                 goal_topic, focus_topic, effort_topic, ogrid_topic, ogrid_threshold):
        """
        Initialize with topic names and ogrid threshold as applicable.
        Defaults are generated at the ROS params level.

        """
        # One-time initializations
        self.revisit_period = 0.05  # s
        self.ogrid = None
        self.ogrid_threshold = float(ogrid_threshold)
        self.state = None
        self.tracking = None
        self.done = True

        # Lil helpers
        self.rostime = lambda: rospy.Time.now().to_sec()
        self.intup = lambda arr: tuple(np.array(arr, dtype=np.int64))
        self.get_hood = lambda img, row, col: img[row-1:row+2, col-1:col+2]

        # Set-up planners
        self.behaviors_list = [car, boat, escape]
        for behavior in self.behaviors_list:
            behavior.planner.set_system(erf=self.erf)
            behavior.planner.set_runtime(sys_time=self.rostime)
            behavior.planner.constraints.set_feasibility_function(self.is_feasible)

        # Initialize resetable stuff
        self.reset()

        # Subscribers
        rospy.Subscriber(odom_topic, Odometry, self.odom_cb)
        rospy.Subscriber(ogrid_topic, OccupancyGrid, self.ogrid_cb)
        rospy.sleep(0.5)

        # Publishers
        self.ref_pub = rospy.Publisher(ref_topic, Odometry, queue_size=1)
        self.path_pub = rospy.Publisher(path_topic, PoseArray, queue_size=3)
        self.tree_pub = rospy.Publisher(tree_topic, PoseArray, queue_size=3)
        self.goal_pub = rospy.Publisher(goal_topic, PoseStamped, queue_size=3)
        self.focus_pub = rospy.Publisher(focus_topic, PointStamped, queue_size=3)
        self.eff_pub = rospy.Publisher(effort_topic, WrenchStamped, queue_size=3)
        self.sampspace_pub = rospy.Publisher(sampspace_topic, PolygonStamped, queue_size=3)
        self.guide_pub = rospy.Publisher(guide_topic, PointStamped, queue_size=3)

        # Actions
        self.move_server = actionlib.SimpleActionServer(move_topic, MoveAction, execute_cb=self.move_cb, auto_start=False)
        self.move_server.start()
        rospy.sleep(1)

        # Timers
        rospy.Timer(rospy.Duration(self.revisit_period), self.publish_ref)
        rospy.Timer(rospy.Duration(self.revisit_period), self.action_check) 

# 需要导入模块: from nav_msgs import msg [as 别名]
# 或者: from nav_msgs.msg import OccupancyGrid [as 别名]
def run(self):
        while self.is_looping():
            navigation_tf_msg = TFMessage()
            try:
                motion_to_robot = almath.Pose2D(self.motion.getRobotPosition(True))
                localization = self.navigation.getRobotPositionInMap()
                exploration_path = self.navigation.getExplorationPath()
            except Exception as e:
                navigation_tf_msg.transforms.append(self.get_navigation_tf(almath.Pose2D()))
                self.publishers["map_tf"].publish(navigation_tf_msg)
                self.rate.sleep()
                continue
            if len(localization) > 0 and len(localization[0]) == 3:
                navigation_to_robot = almath.Pose2D(localization[0][0], localization[0][1], localization[0][2])
                navigation_to_motion = navigation_to_robot * almath.pinv(motion_to_robot)
                navigation_tf_msg.transforms.append(self.get_navigation_tf(navigation_to_motion))
            self.publishers["map_tf"].publish(navigation_tf_msg)
            if len(localization) > 0:
                if self.publishers["uncertainty"].get_num_connections() > 0:
                    uncertainty = Marker()
                    uncertainty.header.frame_id = "/base_footprint"
                    uncertainty.header.stamp = rospy.Time.now()
                    uncertainty.type = Marker.CYLINDER
                    uncertainty.scale = Vector3(localization[1][0], localization[1][1], 0.2)
                    uncertainty.pose.position = Point(0, 0, 0)
                    uncertainty.pose.orientation = self.get_ros_quaternion(
                        almath.Quaternion_fromAngleAndAxisRotation(0, 0, 0, 1))
                    uncertainty.color = ColorRGBA(1, 0.5, 0.5, 0.5)
                    self.publishers["uncertainty"].publish(uncertainty)
            if self.publishers["map"].get_num_connections() > 0:
                aggregated_map = None
                try:
                    aggregated_map = self.navigation.getMetricalMap()
                except Exception as e:
                    print("error " + str(e))
                if aggregated_map is not None:
                    map_marker = OccupancyGrid()
                    map_marker.header.stamp = rospy.Time.now()
                    map_marker.header.frame_id = "/map"
                    map_marker.info.resolution = aggregated_map[0]
                    map_marker.info.width = aggregated_map[1]
                    map_marker.info.height = aggregated_map[2]
                    map_marker.info.origin.orientation = self.get_ros_quaternion(
                        almath.Quaternion_fromAngleAndAxisRotation(-1.57, 0, 0, 1))
                    map_marker.info.origin.position.x = aggregated_map[3][0]
                    map_marker.info.origin.position.y = aggregated_map[3][1]
                    map_marker.data = aggregated_map[4]
                    self.publishers["map"].publish(map_marker)
            if self.publishers["exploration_path"].get_num_connections() > 0:
                path = Path()
                path.header.stamp = rospy.Time.now()
                path.header.frame_id = "/map"
                for node in exploration_path:
                    current_node = PoseStamped()
                    current_node.pose.position.x = node[0]
                    current_node.pose.position.y = node[1]
                    path.poses.append(current_node)
                self.publishers["exploration_path"].publish(path)
            self.rate.sleep()