def console_input(ctx: rs.ContextWrapper): while not ctx.shutting_down(): input_value = input("> ") rawio.say(ctx, input_value)
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")
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()