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