Example #1
0
 def console_input(ctx: rs.ContextWrapper):
     while not ctx.shutting_down():
         input_value = input("> ")
         rawio.say(ctx, input_value)
Example #2
0
def sync_ros_properties(ctx: rs.ContextWrapper):
    """
    State that creates a ROS1-Node, registers all Ros1SubProperties and Ros1PubProperties in ROS1 and keeps them synced
    """
    global global_prop_set, global_node

    # check for ROS1 availability
    if not ROS1_AVAILABLE:
        logger.error(
            "ROS1 is not available, therefore all ROS1-Properties "
            "will be just normal properties without connection to ROS1!")
        return rs.Delete()

    # get config stuff
    node_name = ctx.conf(key=NODE_NAME_CONFIG_KEY)
    if not node_name:
        logger.error(
            f"{NODE_NAME_CONFIG_KEY} is not set. Shutting down ravestate_ros1")
        return rs.Delete()
    spin_frequency = ctx.conf(key=SPIN_FREQUENCY_CONFIG_KEY)
    if spin_frequency is None or spin_frequency < 0:
        logger.error(
            f"{SPIN_FREQUENCY_CONFIG_KEY} is not set or less than 0. Shutting down ravestate_ros1"
        )
        return rs.Delete()
    if spin_frequency == 0:
        spin_sleep_time = 0
    else:
        spin_sleep_time = 1 / spin_frequency

    # Use same node_name if ROS1 was already initialized (i.e. by importing pyroboy)
    if rospy.get_name():
        node_name = rospy.get_name()[1:]  # cut off leading /
    # init ROS1
    #rospy.init_node(node_name, disable_signals=True)
    global global_prop_set
    # current_props: hash -> Subscriber/Publisher/ServiceProxy
    current_props: Dict = dict()

    # ROS1-Context Sync Loop
    while not ctx.shutting_down() and not rospy.core.is_shutdown():
        # remove deleted props
        removed_props = current_props.keys() - global_prop_set
        for prop_hash in removed_props:
            item = current_props[prop_hash]
            item.unregister()
            current_props.pop(prop_hash)

        # add new props
        new_props = global_prop_set - current_props.keys()
        for prop in new_props:
            # register subscribers in ROS1
            if isinstance(prop, Ros1SubProperty):
                # register in context
                @rs.receptor(ctx_wrap=ctx, write=prop.id())
                def ros_to_ctx_callback(ctx, msg, prop_name: str):
                    ctx[prop_name] = msg

                prop.ros_to_ctx_callback = ros_to_ctx_callback
                prop.subscriber = rospy.Subscriber(
                    prop.topic, prop.msg_type, prop.ros_subscription_callback)
                current_props[prop.__hash__()] = prop.subscriber
            # register publishers in ROS1
            if isinstance(prop, Ros1PubProperty):
                prop.publisher = rospy.Publisher(prop.topic,
                                                 prop.msg_type,
                                                 queue_size=prop.queue_size)
                current_props[prop.__hash__()] = prop.publisher
            # register clients in ROS1
            if isinstance(prop, Ros1CallProperty):
                prop.client = rospy.ServiceProxy(prop.service_name,
                                                 prop.service_type)
                current_props[prop.__hash__()] = prop.client

            # replace prop with hash in global_props
            global_prop_set.remove(prop)
            global_prop_set.add(prop.__hash__())

        rospy.rostime.wallsleep(spin_sleep_time)

    rospy.signal_shutdown("ravestate_ros1 is shutting down")
Example #3
0
def sync_ros_properties(ctx: rs.ContextWrapper):
    """
    State that creates a ROS2-Node, registers all Ros2SubProperties and Ros2PubProperties in ROS2 and keeps them synced
    """
    global global_prop_set, global_node

    # check for ROS2 availability
    if not ROS2_AVAILABLE:
        logger.error(
            "ROS2 is not available, therefore all ROS2-Properties "
            "will be just normal properties without connection to ROS2!")
        return rs.Delete()

    # get config stuff
    node_name = ctx.conf(key=NODE_NAME_CONFIG_KEY)
    if not node_name:
        logger.error(
            f"{NODE_NAME_CONFIG_KEY} is not set. Shutting down ravestate_ros2")
        return rs.Delete()
    spin_frequency = ctx.conf(key=SPIN_FREQUENCY_CONFIG_KEY)
    if spin_frequency is None or spin_frequency < 0:
        logger.error(
            f"{SPIN_FREQUENCY_CONFIG_KEY} is not set or less than 0. Shutting down ravestate_ros2"
        )
        return rs.Delete()
    if spin_frequency == 0:
        spin_sleep_time = 0
    else:
        spin_sleep_time = 1 / spin_frequency

    # init ROS
    if not rclpy.ok():
        rclpy.init()
    if not global_node:
        global_node = rclpy.create_node(node_name)

    global global_prop_set
    # current_props: hash -> subscription/publisher
    current_props: Dict = dict()

    # ROS-Context Sync Loop
    while not ctx.shutting_down():
        # remove deleted props
        removed_props = current_props.keys() - global_prop_set
        for prop_hash in removed_props:
            item = current_props[prop_hash]
            if isinstance(item, rclpy.subscription.Subscription):
                global_node.destroy_subscription(item)
            elif isinstance(item, rclpy.publisher.Publisher):
                global_node.destroy_publisher(item)
            elif isinstance(item, rclpy.client.Client):
                global_node.destroy_client(item)
            current_props.pop(prop_hash)

        # add new props
        new_props = global_prop_set - current_props.keys()
        for prop in new_props:
            # register subscribers in ROS
            if isinstance(prop, Ros2SubProperty):
                # register in context
                @rs.receptor(ctx_wrap=ctx, write=prop.id())
                def ros_to_ctx_callback(ctx, msg, prop_name: str):
                    ctx[prop_name] = msg

                prop.ros_to_ctx_callback = ros_to_ctx_callback
                prop.subscription = global_node.create_subscription(
                    prop.msg_type, prop.topic, prop.ros_subscription_callback)
                current_props[prop.__hash__()] = prop.subscription
            # register publishers in ROS
            if isinstance(prop, Ros2PubProperty):
                prop.publisher = global_node.create_publisher(
                    prop.msg_type, prop.topic)
                current_props[prop.__hash__()] = prop.publisher
            # register clients in ROS
            if isinstance(prop, Ros2CallProperty):
                prop.client = global_node.create_client(
                    prop.service_type, prop.service_name)
                current_props[prop.__hash__()] = prop.client

            # replace prop with hash in global_props
            global_prop_set.remove(prop)
            global_prop_set.add(prop.__hash__())

        # spin once
        rclpy.spin_once(global_node, timeout_sec=0)
        time.sleep(spin_sleep_time)

    global_node.destroy_node()
    rclpy.shutdown()