def main():
rclpy.init()
node = Node("talker")
publeft = node.create_publisher(Float32, 'robot0/left_wheel')
pubright = node.create_publisher(Float32, 'robot0/right_wheel')
simTime = SimTimer(True, "veranda/timestamp", node)
# Factor to scale down speed by
speedScale = 2
# Tick time at 10 hz
dt = 0.1
def cb():
# Calculate wheel velocities for current time
phi1, phi2 = DD_IK(x_t, y_t, simTime.global_time() + 2*math.pi, speedScale)
print(phi1, phi2)
# Publish velocities
publishWheelVelocity(publeft, pubright, phi1, phi2)
simTime.create_timer(dt, cb)
rclpy.spin(node)
node.destroy_node()
rclpy.shutdown()
def publisher(
node: Node,
message_type: MsgType,
topic_name: str,
values: dict,
period: float,
print_nth: int,
once: bool,
qos_profile: QoSProfile,
) -> Optional[str]:
"""Initialize a node with a single publisher and run its publish loop (maybe only once)."""
msg_module = get_message(message_type)
values_dictionary = yaml.safe_load(values)
if not isinstance(values_dictionary, dict):
return 'The passed value needs to be a dictionary in YAML format'
publisher_callbacks = PublisherEventCallbacks(
incompatible_qos=handle_incompatible_qos_event)
try:
pub = node.create_publisher(msg_module,
topic_name,
qos_profile,
event_callbacks=publisher_callbacks)
except UnsupportedEventTypeError:
pub = node.create_publisher(msg_module, topic_name, qos_profile)
msg = msg_module()
try:
set_message_fields(msg, values_dictionary)
except Exception as e:
return 'Failed to populate field: {0}'.format(e)
print('publisher: beginning loop')
count = 0
def timer_callback():
nonlocal count
count += 1
if print_nth and count % print_nth == 0:
print('publishing #%d: %r\n' % (count, msg))
pub.publish(msg)
timer = node.create_timer(period, timer_callback)
if once:
rclpy.spin_once(node)
time.sleep(
0.1) # make sure the message reaches the wire before exiting
else:
rclpy.spin(node)
node.destroy_timer(timer)
class Pause(object):
"""
Always go in and out of pause by manual penalty.
Go in and out of pause by game controller, if manual penalty is not active.
"""
def __init__(self):
rclpy.init(args=None)
self.node = Node("Pause")
self.penalty_manual = False
self.game_controller_penalty = False
self.pause = False
self.manual_penalize_service = self.node.create_service(
ManualPenalize, "manual_penalize", self.manual_update)
self.pause_publisher = self.node.create_publisher(Bool, "pause",
10) # todo latch
self.speak_publisher = self.node.create_publisher(Audio, "speak", 10)
while rclpy.ok():
try:
rclpy.spin_once(self.node)
except (ExternalShutdownException, KeyboardInterrupt):
exit(0)
def manual_update(self, req):
if req.penalize == 0:
# off
self.penalty_manual = False
elif req.penalize == 1:
# on
self.penalty_manual = True
elif req.penalize == 2:
# switch
self.penalty_manual = not self.penalty_manual
else:
self.node.get_logger().error(
"Manual penalize call with unspecified request")
self.set_pause(self.penalty_manual)
return True
def set_pause(self, state):
self.pause = state
if state:
text = "Pause!"
else:
text = "Unpause!"
self.node.get_logger().warn(text)
speak(text, self.speak_publisher, speaking_active=True, priority=90)
self.pause_publisher.publish(Bool(data=state))
def main():
rclpy.init()
node = Node("talker")
pub = node.create_publisher(Pose2D, 'robot0/target_velocity')
simTime = SimTimer(True, "veranda/timestamp", node)
# Factor to scale down speed by
speedScale = 10
# Tick time at 10 hz
dt = 0.1
def cb():
# Calculate wheel velocities for current time
x, y = speedScale * math.sin(
simTime.global_time()), speedScale * 2 * math.cos(
2 * simTime.global_time())
t = 0.
print(x, y)
# Publish velocities
publishTargetVelocity(pub, x, y, t)
simTime.create_timer(dt, cb)
rclpy.spin(node)
node.destroy_node()
rclpy.shutdown()
def __init__(self, node: Node, params: dict):
self._node = node
self.topic = params['topic']
self.lin_throttle = params['throttle']['lin_throttle_ctrl']
self.ang_throttle = params['throttle']['ang_throttle_ctrl']
self._is_turbo = False
self._before_turbo = 1.0
self.x = params['data']['linear']['x']
self.y = params['data']['linear']['y']
self.z = params['data']['linear']['z']
self.roll = params['data']['angular']['x']
self.pitch = params['data']['angular']['y']
self.yaw = params['data']['angular']['z']
self._HALT = Twist()
self._last_message_was_halt = False
self._lin_throttle_increment = params['throttle']['lin_increment']
self._ang_throttle_increment = params['throttle']['ang_increment']
self._last_lin_throttle_input = 0
self._lin_throttle_coef = 1.0
self._last_ang_throttle_input = 0
self._ang_throttle_coef = 1.0
try:
self.publish_multiple_halts = params['publish_multiple_halt']
except:
self.publish_multiple_halts = True
self._publisher = node.create_publisher(Twist, self.topic, 10)
def mainLoop(device):
#Gripper is a 2F with a TCP connection
gripper = robotiq_2f_gripper_control.baseRobotiq2FGripper.robotiqbaseRobotiq2FGripper()
gripper.client = robotiq_modbus_rtu.comModbusRtu.communication()
#We connect to the address received as an argument
gripper.client.connectToDevice(device)
node = Node('robotiq2FGripper')
#The Gripper status is published on the topic named 'Robotiq2FGripperRobotInput'
pub = node.create_publisher(inputMsg, 'Robotiq2FGripperRobotInput', 1)
#The Gripper command is received from the topic named 'Robotiq2FGripperRobotOutput'
sud = node.create_subscription(outputMsg, 'Robotiq2FGripperRobotOutput', gripper.refreshCommand, 1)
#We loop
while rclpy.ok():
#Get and publish the Gripper status
status = gripper.getStatus()
pub.publish(status)
#Wait a little
#rospy.sleep(0.05)
#Send the most recent command
gripper.sendCommand()
#Wait a little
#rospy.sleep(0.05)
node.destroy_node()
rclpy.shutdown()
def publisher(
node: Node,
message_type: MsgType,
topic_name: str,
values: dict,
period: float,
print_nth: int,
times: int,
wait_matching_subscriptions: int,
qos_profile: QoSProfile,
keep_alive: float,
) -> Optional[str]:
"""Initialize a node with a single publisher and run its publish loop (maybe only once)."""
try:
msg_module = get_message(message_type)
except (AttributeError, ModuleNotFoundError, ValueError):
raise RuntimeError('The passed message type is invalid')
values_dictionary = yaml.safe_load(values)
if not isinstance(values_dictionary, dict):
return 'The passed value needs to be a dictionary in YAML format'
pub = node.create_publisher(msg_module, topic_name, qos_profile)
times_since_last_log = 0
while pub.get_subscription_count() < wait_matching_subscriptions:
# Print a message reporting we're waiting each 1s, check condition each 100ms.
if not times_since_last_log:
print(
f'Waiting for at least {wait_matching_subscriptions} matching subscription(s)...'
)
times_since_last_log = (times_since_last_log + 1) % 10
time.sleep(0.1)
msg = msg_module()
try:
set_message_fields(msg, values_dictionary)
except Exception as e:
return 'Failed to populate field: {0}'.format(e)
print('publisher: beginning loop')
count = 0
def timer_callback():
nonlocal count
count += 1
if print_nth and count % print_nth == 0:
print('publishing #%d: %r\n' % (count, msg))
pub.publish(msg)
timer_callback()
if times != 1:
timer = node.create_timer(period, timer_callback)
while times == 0 or count < times:
rclpy.spin_once(node)
# give some time for the messages to reach the wire before exiting
time.sleep(keep_alive)
node.destroy_timer(timer)
else:
# give some time for the messages to reach the wire before exiting
time.sleep(keep_alive)
def main():
args = sys.argv
if len(args) != 2 and len(args) != 3:
print("Usage: joystick_differential {topic-in} [topic-out]")
print("Joystick should be {topic-in}/joystick")
print(
"Wheels will be {topic-out}/steer, {topic-out}/left_wheel, and {topic-out}/right_wheel"
)
print("If no {topic-out} given, {topic-in} will be used for both")
return
channelin = channelout = args[1]
if len(args) == 3:
channelout = args[2]
channelin = channelin + "/joystick"
rclpy.init(args=args)
node = Node("joystick_differential")
publeft = node.create_publisher(Float32, channelout + "/left_wheel")
pubright = node.create_publisher(Float32, channelout + "/right_wheel")
pubsteer = node.create_publisher(Float32, channelout + "/steer")
def joystick_callback(msg):
if len(msg.axes) < 2:
print("Not enough joystick axes")
return
speed = msg.axes[1]
steer = -msg.axes[0]
phi1 = phi2 = 1 / R * speed * SPEED
phi3 = math.pi / 4.0 * steer
publishWheelVelocity(publeft, pubright, phi1, phi2)
publishWheelSteer(pubsteer, phi3)
node.create_subscription(Joy, channelin, joystick_callback)
rclpy.spin(node)
node.destroy_node()
rclpy.shutdown()
def rosinit(self):
node_name = 'circle'
rclpy.init()
node = Node(node_name)
msg = Float32()
print('Spinning: {}'.format(node_name))
self.publeft = node.create_publisher(Float32, '/wheel_left')
self.pubright = node.create_publisher(Float32, '/wheel_right')
if self.problem == 'triangle':
self.goal_pt = next(self.tri_pts)
self.goal_pt = next(self.tri_pts)
self.goal_theta = next(self.tri_thetas)
self.goal_theta = next(self.tri_thetas)
msg.data = 1.0
self.publeft.publish(msg)
msg.data = -msg.data
self.pubright.publish(msg)
node.create_subscription(Pose2D, '/GPS', self.tri_callback)
elif self.problem == 'square':
self.goal_pt = next(self.sqr_pts)
self.goal_pt = next(self.sqr_pts)
self.goal_theta = next(self.sqr_thetas)
self.goal_theta = next(self.sqr_thetas)
msg.data = 1.0
self.publeft.publish(msg)
msg.data = -msg.data
self.pubright.publish(msg)
node.create_subscription(Pose2D, '/GPS', self.sqr_callback)
else:
# Circle
msg.data = 5.0
self.publeft.publish(msg)
msg.data = msg.data * (15.0 + 1.2) / 15
self.pubright.publish(msg)
try:
rclpy.spin(node)
except KeyboardInterrupt:
print('Shutting down...')
msg.data = 0.0
self.publeft.publish(msg)
self.pubright.publish(msg)
node.destroy_node()
rclpy.shutdown()
def __init__(self, node: Node) -> None:
self.node = node
self.waypoints = []
self.balls = [None for i in range(10)]
self.tfBuffer = tf2_ros.Buffer()
self.listener = tf2_ros.TransformListener(self.tfBuffer, self.node)
self.pubBackDoor = node.create_publisher(Wrench, "/force_back_door", qos_profile_services_default)
self.pubFrontDoor = node.create_publisher(Wrench, "/force_front_door", qos_profile_services_default)
self.pubVel = node.create_publisher(Float32, "/vel", qos_profile_services_default)
self.pubPointA = node.create_publisher(Point, "/pointA", qos_profile_services_default)
self.pubPointB = node.create_publisher(Point, "/pointB", qos_profile_services_default)
self.pubTarget = node.create_publisher(Pose2D, "/objectif", qos_profile_services_default)
node.create_subscription(Float32MultiArray, "/balls_coords", self.__balls_callback, 10)
node.create_subscription(Float64MultiArray, "/waypoints", self.__wp_callback, qos_profile_sensor_data)
self.newWaypoints = False
def decorated_function():
#SETUP
print("Setup fixture")
rclpy.init()
node = Node('minimal_publisher')
pub1 = node.create_publisher(String, 'topic1', latching_qos)
msg1 = String()
msg1.data = "hello1"
node.get_logger().info("Publishing \"{}\" on topic \"{}\"".format("hello1", "topic1"))
pub1.publish(msg1)
pub2 = node.create_publisher(String, 'topic2', latching_qos)
msg2 = String()
msg2.data = "hello2"
node.get_logger().info("Publishing \"{}\" on topic \"{}\"".format("hello2", "topic2"))
pub2.publish(msg2)
pub3 = node.create_publisher(String, 'topic3', latching_qos)
msg3 = String()
msg3.data = "hello3"
node.get_logger().info("Publishing \"{}\" on topic \"{}\"".format("hello3", "topic3"))
pub3.publish(msg3)
exe = SingleThreadedExecutor()
exe.add_node(node)
future_message = Future()
#TEST
result = function(future_message, exe)
#TEARDOWN
print("Teardown fixture")
node.destroy_node()
rclpy.shutdown()
time.sleep(0.2)
return result
def __init__(self, node: Node, connector: Connector,
param: NodeParameters):
self.node = node
self.con = connector
self.param = param
prefix = self.param.ros_topic_prefix.value
QoSProf = QoSProfile(depth=10)
# create topic publishers:
self.pub_imu_raw = node.create_publisher(Imu, prefix + 'imu_raw',
QoSProf)
self.pub_imu = node.create_publisher(Imu, prefix + 'imu', QoSProf)
self.pub_mag = node.create_publisher(MagneticField, prefix + 'mag',
QoSProf)
self.pub_temp = node.create_publisher(Temperature, prefix + 'temp',
QoSProf)
self.pub_calib_status = node.create_publisher(String,
prefix + 'calib_status',
QoSProf)
def __init__(self, name: str, config: typing.Dict[str, typing.Any], node: Node) -> None:
super().__init__(name, config, 'deadman_buttons', 'deadman_axes')
self.name = name
self.topic_type = get_interface_type(config['interface_type'], 'msg')
# A 'message_value' is a fixed message that is sent in response to an activation. It is
# mutually exclusive with an 'axis_mapping'.
self.msg_value = None
if 'message_value' in config:
msg_config = config['message_value']
# Construct the fixed message and try to fill it in. This message will be reused
# during runtime, and has the side benefit of giving the user early feedback if the
# config can't work.
self.msg_value = self.topic_type()
for target, param in msg_config.items():
set_member(self.msg_value, target, param['value'])
# An 'axis_mapping' takes data from one part of the message and scales and offsets it to
# publish if an activation happens. This is typically used to take joystick analog data
# and republish it as a cmd_vel. It is mutually exclusive with a 'message_value'.
self.axis_mappings = {}
if 'axis_mappings' in config:
self.axis_mappings = config['axis_mappings']
# Now check that the mappings have all of the required configuration.
for mapping, values in self.axis_mappings.items():
if 'axis' not in values and 'button' not in values and 'value' not in values:
raise JoyTeleopException("Axis mapping for '{}' must have an axis, button, "
'or value'.format(name))
if 'axis' in values:
if 'offset' not in values:
raise JoyTeleopException("Axis mapping for '{}' must have an offset"
.format(name))
if 'scale' not in values:
raise JoyTeleopException("Axis mapping for '{}' must have a scale"
.format(name))
if self.msg_value is None and not self.axis_mappings:
raise JoyTeleopException("No 'message_value' or 'axis_mappings' "
"configured for command '{}'".format(name))
if self.msg_value is not None and self.axis_mappings:
raise JoyTeleopException("Only one of 'message_value' or 'axis_mappings' "
"can be configured for command '{}'".format(name))
qos = rclpy.qos.QoSProfile(history=rclpy.qos.QoSHistoryPolicy.KEEP_LAST,
depth=1,
reliability=rclpy.qos.QoSReliabilityPolicy.RELIABLE,
durability=rclpy.qos.QoSDurabilityPolicy.VOLATILE)
self.pub = node.create_publisher(self.topic_type, config['topic_name'], qos)
def run():
node = Node("testHeadBehaviour")
pub_ball = node.create_publisher(BallInImage, "ball_in_image", 1)
pub_hmg = node.create_publisher(JointTrajectory, "head_motor_goals", 1)
hmg = JointTrajectory()
goal = JointTrajectoryPoint()
goal.positions = [0, 0]
goal.velocities = [0, 0]
hmg.points = [goal]
counter = 320
direction = 1
node.get_logger().info("Create Test")
rclpy.init(args=None)
pub_hmg.publish(hmg)
rate = node.create_rate(4)
node.get_logger().debug("Laeuft...")
while rclpy.ok():
# Ball in Image
ball = BallInImage()
ball.center.x = counter
if(counter > 340 or counter < 300):
direction *= -1
counter += direction
else:
counter += direction
ball.center.y = 200
ball.diameter = 10
ball.confidence = 1
balls = BallInImageArray()
balls.candidates.append(ball)
pub_ball.publish(balls)
node.get_logger().info("Published ball: %s" % counter)
rate.sleep()
def __init__(self):
_node = Node("tf2twist")
qos_profile = QoSProfile(depth=10)
# loop_rate = _node.create_rate(100)
tfBuffer = Buffer()
listener = TransformListener(tfBuffer, _node, qos=qos_profile)
twist_data = geometry_msgs.msg.Twist()
pub_cmd_vel = _node.create_publisher(geometry_msgs.msg.Twist,
"cmd_vel", qos_profile)
# self.transfromstamped =
try:
while rclpy.ok():
rclpy.spin_once(_node)
now = _node.get_clock().now() - rclpy.duration.Duration(
seconds=0, nanoseconds=1000000)
try:
trans = tfBuffer.lookup_transform(
'odom_frame', 'base_link', now,
rclpy.duration.Duration(seconds=0, nanoseconds=0))
# print(trans)
# print(trans.transform.rotation.x)
roll, pitch, yaw = self.quaternion_to_euler(
trans.transform.rotation.x, trans.transform.rotation.y,
trans.transform.rotation.z, trans.transform.rotation.w)
# print(yaw)
if (yaw > 0.3):
twist_data.angular.z = 70.0 #yaw*125*1.3
elif (yaw < -0.3):
twist_data.angular.z = -70.0 #yaw*125*1.3
else:
twist_data.angular.z = 0.0
pub_cmd_vel.publish(twist_data)
except (LookupException, LookupError, ConnectionAbortedError,
ConnectionError, ConnectionRefusedError,
ConnectionResetError, ExtrapolationException,
ConnectivityException) as e:
# print(e)
pass
except (KeyboardInterrupt):
pass
def __init__(self,
node: Node,
qos: Optional[Union[QoSProfile, int]] = None) -> None:
"""
.. function:: __init__(node, qos=None)
Constructor.
:param node: The ROS2 node.
:param qos: A QoSProfile or a history depth to apply to the publisher.
"""
if qos is None:
qos = QoSProfile(depth=100)
self.pub_tf = node.create_publisher(TFMessage, "/tf", qos)
def __init__(self, node: Node, qos: Optional[Union[QoSProfile, int]] = None) -> None:
"""
Constructor.
:param node: The ROS2 node.
:param qos: A QoSProfile or a history depth to apply to the publisher.
"""
if qos is None:
qos = QoSProfile(
depth=1,
durability=DurabilityPolicy.TRANSIENT_LOCAL,
history=HistoryPolicy.KEEP_LAST,
)
self.pub_tf = node.create_publisher(TFMessage, "/tf_static", qos)
class TestRosListener(unittest.TestCase):
def setUp(self):
rclpy.init()
self.node = Node('fuzzer_node')
self.pub = self.node.create_publisher(ParameterEvent, '/chatter')
self.health_checker = FuzzedNodeHandler('/listener')
def tearDown(self):
rclpy.shutdown('Shutting down fuzzer node')
@given(message=map_ros_types(String))
def test_message(self, message):
self.pub.publish(message)
assert self.health_checker.check_if_alive()
def main():
rclpy.init()
node = Node('Controller')
pub = node.create_publisher(String, 'DIRECTION', 10)
print("W--UP\nA--LEFT\nD--RIGHT\nS--DOWN\nQ--STOP")
ip = input()
msg = String()
while ip != 'Z':
if (ip == "W" or ip == "S" or ip == "A" or ip == "D" or ip == "Q"):
msg.data = ip
pub.publish(msg)
else:
print("W--UP\nA--LEFT\nD--RIGHT\nS--DOWN\nQ--STOP")
ip = input()
node.destroy_node()
rclpy.shutdown()
def create_lifecycle_publisher(self, *args, **kwargs):
# TODO(ivanpauno): Should we override lifecycle publisher?
# There is an issue with python using the overridden method
# when creating publishers for builitin publishers (like parameters events).
# We could override them after init, similar to what we do to override publish()
# in LifecycleNode.
# Having both options seem fine.
if 'publisher_class' in kwargs:
raise TypeError(
"create_publisher() got an unexpected keyword argument 'publisher_class'"
)
pub = Node.create_publisher(self,
*args,
**kwargs,
publisher_class=LifecyclePublisher)
self._managed_entities.add(pub)
return pub
def publisher(
node: Node,
message_type: MsgType,
topic_name: str,
values: dict,
period: float,
print_nth: int,
times: int,
qos_profile: QoSProfile,
keep_alive: float,
) -> Optional[str]:
"""Initialize a node with a single publisher and run its publish loop (maybe only once)."""
msg_module = get_message(message_type)
values_dictionary = yaml.safe_load(values)
if not isinstance(values_dictionary, dict):
return 'The passed value needs to be a dictionary in YAML format'
pub = node.create_publisher(msg_module, topic_name, qos_profile)
msg = msg_module()
try:
set_message_fields(msg, values_dictionary)
except Exception as e:
return 'Failed to populate field: {0}'.format(e)
print('publisher: beginning loop')
count = 0
def timer_callback():
nonlocal count
count += 1
if print_nth and count % print_nth == 0:
print('publishing #%d: %r\n' % (count, msg))
pub.publish(msg)
timer = node.create_timer(period, timer_callback)
while times == 0 or count < times:
rclpy.spin_once(node)
# give some time for the messages to reach the wire before exiting
time.sleep(keep_alive)
node.destroy_timer(timer)
def publisher(args=None):
"""Main loop which requests new commands and publish them on the Robotiq2FGripperRobotOutput topic."""
rclpy.init(args=args)
node = Node('Robotiq2FGripperSimpleController')
pub = node.create_publisher(outputMsg, 'Robotiq2FGripperRobotOutput', 1)
command = outputMsg()
try:
while rclpy.ok():
command = genCommand(askForCommand(command), command)
pub.publish(command)
sleep(0.1)
except KeyboardInterrupt:
pass
node.destroy_node()
rclpy.shutdown()
class HardwareControlManager:
def __init__(self):
rclpy.init(args=None)
self.node = Node("hcm",
allow_undeclared_parameters=True,
automatically_declare_parameters_from_overrides=True)
multi_executor = MultiThreadedExecutor()
multi_executor.add_node(self.node)
self.spin_thread = threading.Thread(target=multi_executor.spin,
args=(),
daemon=True)
#self.spin_thread = threading.Thread(target=rclpy.spin, args=(self.node,), daemon=True)
self.spin_thread.start()
self.blackboard = None
# --- Initialize Node ---
# Otherwise messages will get lost, bc the init is not finished
self.node.get_clock().sleep_for(Duration(seconds=0.1))
self.node.get_logger().debug("Starting hcm")
self.simulation_active = self.node.get_parameter("simulation_active")
# dsd
self.blackboard = HcmBlackboard(self.node)
self.blackboard.animation_action_client = ActionClient(
self.node, PlayAnimation, 'animation')
self.blackboard.dynup_action_client = ActionClient(
self.node, Dynup, 'dynup')
dirname = os.path.dirname(os.path.realpath(__file__)) + "/hcm_dsd"
self.dsd = DSD(self.blackboard, "debug/dsd/hcm", node=self.node)
self.dsd.register_actions(os.path.join(dirname, 'actions'))
self.dsd.register_decisions(os.path.join(dirname, 'decisions'))
self.dsd.load_behavior(os.path.join(dirname, 'hcm.dsd'))
self.hcm_deactivated = False
# Publisher / subscriber
self.joint_goal_publisher = self.node.create_publisher(
JointCommand, 'DynamixelController/command', 1)
self.hcm_state_publisher = self.node.create_publisher(
RobotControlState, 'robot_state', 1) # todo latch
self.blackboard.speak_publisher = self.node.create_publisher(
Audio, 'speak', 1)
# important to make sure the connection to the speaker is established, for next line
self.node.get_clock().sleep_for(Duration(seconds=0.1))
speak("Starting hcm", self.blackboard.speak_publisher, priority=50)
self.node.create_subscription(Imu, "imu/data", self.update_imu, 1)
self.node.create_subscription(FootPressure,
"foot_pressure_left/filtered",
self.update_pressure_left, 1)
self.node.create_subscription(FootPressure,
"foot_pressure_right/filtered",
self.update_pressure_right, 1)
self.node.create_subscription(JointCommand, "walking_motor_goals",
self.walking_goal_callback, 1)
self.node.create_subscription(AnimationMsg, "animation",
self.animation_callback, 1)
self.node.create_subscription(JointCommand, "dynup_motor_goals",
self.dynup_callback, 1)
self.node.create_subscription(JointCommand, "head_motor_goals",
self.head_goal_callback, 1)
self.node.create_subscription(JointCommand, "record_motor_goals",
self.record_goal_callback, 1)
self.node.create_subscription(JointCommand, "kick_motor_goals",
self.kick_goal_callback, 1)
self.node.create_subscription(Bool, "pause", self.pause, 1)
self.node.create_subscription(JointState, "joint_states",
self.joint_state_callback, 1)
self.node.create_subscription(PointStamped, "cop_l", self.cop_l_cb, 1)
self.node.create_subscription(PointStamped, "cop_r", self.cop_r_cb, 1)
self.node.create_subscription(Bool, "core/power_switch_status",
self.power_cb, 1)
self.node.create_subscription(Bool, "hcm_deactivate",
self.deactivate_cb, 1)
self.node.create_subscription(DiagnosticArray, "diagnostics_agg",
self.blackboard.diag_cb, 1)
self.node.add_on_set_parameters_callback(self.on_set_parameters)
self.main_loop()
def deactivate_cb(self, msg):
self.hcm_deactivated = msg.data
def pause(self, msg):
""" Updates the stop state for the state machine"""
self.blackboard.stopped = msg.data
def update_imu(self, msg):
"""Gets new IMU values and computes the smoothed values of these"""
self.blackboard.last_imu_update_time = msg.header.stamp
self.blackboard.accel = numpy.array([
msg.linear_acceleration.x, msg.linear_acceleration.y,
msg.linear_acceleration.z
])
self.blackboard.gyro = numpy.array([
msg.angular_velocity.x, msg.angular_velocity.y,
msg.angular_velocity.z
])
self.blackboard.quaternion = numpy.array(([
msg.orientation.x, msg.orientation.y, msg.orientation.z,
msg.orientation.w
]))
self.blackboard.smooth_gyro = numpy.multiply(
self.blackboard.smooth_gyro, 0.95) + numpy.multiply(
self.blackboard.gyro, 0.05)
self.blackboard.smooth_accel = numpy.multiply(
self.blackboard.smooth_accel, 0.99) + numpy.multiply(
self.blackboard.accel, 0.01)
self.blackboard.not_much_smoothed_gyro = numpy.multiply(
self.blackboard.not_much_smoothed_gyro, 0.5) + numpy.multiply(
self.blackboard.gyro, 0.5)
self.blackboard.imu_msg = msg
def update_pressure_left(self, msg):
"""Gets new pressure values and writes them to the blackboard"""
self.blackboard.last_pressure_update_time = msg.header.stamp
self.blackboard.pressures[0] = msg.left_front
self.blackboard.pressures[1] = msg.left_back
self.blackboard.pressures[2] = msg.right_front
self.blackboard.pressures[3] = msg.right_back
def update_pressure_right(self, msg):
"""Gets new pressure values and writes them to the blackboard"""
self.blackboard.last_pressure_update_time = msg.header.stamp
self.blackboard.pressures[4] = msg.left_front
self.blackboard.pressures[5] = msg.left_back
self.blackboard.pressures[6] = msg.right_front
self.blackboard.pressures[7] = msg.right_back
def on_set_parameters(self, config, level):
""" Dynamic reconfigure of the fall checker values."""
# just pass on to the StandupHandler, as all the variables are located there
self.blackboard.fall_checker.update_reconfigurable_values(
config, level)
self.blackboard.pickup_accel_threshold = config[
"pick_up_accel_threshold"]
return config
def walking_goal_callback(self, msg):
self.blackboard.last_walking_goal_time = self.node.get_clock().now()
if self.blackboard.current_state in [
RobotControlState.CONTROLLABLE, RobotControlState.WALKING
]:
self.joint_goal_publisher.publish(msg)
def dynup_callback(self, msg):
if self.blackboard.current_state in [
RobotControlState.STARTUP, RobotControlState.FALLEN,
RobotControlState.GETTING_UP, RobotControlState.CONTROLLABLE
]:
self.joint_goal_publisher.publish(msg)
def head_goal_callback(self, msg):
if self.blackboard.current_state in [
RobotControlState.CONTROLLABLE, RobotControlState.WALKING
]:
# we can move our head
self.joint_goal_publisher.publish(msg)
def record_goal_callback(self, msg):
if msg.joint_names == []:
# record tells us that its finished
self.blackboard.record_active = False
else:
self.blackboard.record_active = True
self.joint_goal_publisher.publish(msg)
def kick_goal_callback(self, msg):
if self.blackboard.current_state in [
RobotControlState.KICKING, RobotControlState.CONTROLLABLE
]:
# we can perform a kick
self.joint_goal_publisher.publish(msg)
def animation_callback(self, msg):
""" The animation server is sending us goal positions for the next keyframe"""
self.blackboard.last_animation_goal_time = msg.header.stamp
if msg.request:
self.node.get_logger().info(
"Got Animation request. HCM will try to get controllable now.")
# animation has to wait
# dsd should try to become controllable
self.blackboard.animation_requested = True
return
if msg.first:
if msg.hcm:
# coming from ourselves
# we don't have to do anything, since we must be in the right state
pass
else:
# coming from outside
# check if we can run an animation now
if self.blackboard.current_state != RobotControlState.CONTROLLABLE:
self.node.get_logger().warn(
"HCM is not controllable, animation refused.")
return
else:
# we're already controllable, go to animation running
self.blackboard.external_animation_running = True
if msg.last:
if msg.hcm:
# This was an animation from the DSD
self.blackboard.hcm_animation_finished = True
pass
else:
# this is the last frame, we want to tell the DSD that we're finished with the animations
self.blackboard.external_animation_running = False
if msg.position is None:
# probably this was just to tell us we're finished
# we don't need to set another position to the motors
return
# forward positions to motors, if some where transmitted
if len(
msg.position.points
) > 0 and self.blackboard.current_state != RobotControlState.GETTING_UP:
out_msg = JointCommand()
out_msg.positions = msg.position.points[0].positions
out_msg.joint_names = msg.position.joint_names
out_msg.accelerations = [-1.0] * len(out_msg.joint_names)
out_msg.velocities = [-1.0] * len(out_msg.joint_names)
out_msg.max_currents = [-1.0] * len(out_msg.joint_names)
if msg.position.points[0].effort:
out_msg.max_currents = [
-x for x in msg.position.points[0].effort
]
if self.blackboard.shut_down_request:
# there are sometimes transmission errors during shutdown due to race conditions
# there is nothing we can do so just ignore the errors in this case
try:
self.joint_goal_publisher.publish(out_msg)
except:
pass
else:
self.joint_goal_publisher.publish(out_msg)
def joint_state_callback(self, msg: JointState):
self.blackboard.last_motor_update_time = msg.header.stamp
self.blackboard.previous_joint_state = self.blackboard.current_joint_state
self.blackboard.current_joint_state = msg
def cop_l_cb(self, msg):
self.blackboard.cop_l_msg = msg
def cop_r_cb(self, msg):
self.blackboard.cop_r_msg = msg
def power_cb(self, msg):
self.blackboard.is_power_on = msg.data
def main_loop(self):
""" Keeps updating the DSD and publish its current state.
All the forwarding of joint goals is directly done in the callbacks to reduce latency. """
last_loop_start_time = self.node.get_clock().now()
while rclpy.ok() and not self.blackboard.shut_down_request:
try:
loop_start_time = self.node.get_clock().now()
#can happen in simulation due to bad implementation in rclpy
if (last_loop_start_time != loop_start_time):
last_loop_start_time = loop_start_time
if self.hcm_deactivated:
self.blackboard.current_state = RobotControlState.CONTROLLABLE
msg = RobotControlState()
msg.state = self.blackboard.current_state
self.hcm_state_publisher.publish(msg)
else:
self.blackboard.current_time = self.node.get_clock(
).now()
try:
self.dsd.update()
msg = RobotControlState()
msg.state = self.blackboard.current_state
self.hcm_state_publisher.publish(msg)
except IndexError:
# this error will happen during shutdown procedure, just ignore it
pass
self.node.get_clock().sleep_until(loop_start_time +
Duration(seconds=1 / 500.0))
except (ExternalShutdownException, KeyboardInterrupt):
if not self.simulation_active:
self.on_shutdown_hook()
exit(0)
if not self.simulation_active:
self.on_shutdown_hook()
self.spin_thread.join()
def on_shutdown_hook(self):
if not self.blackboard:
return
# we got external shutdown, tell it to the DSD, it will handle it
self.blackboard.shut_down_request = True
self.node.get_logger().warn(
"You're stopping the HCM. The robot will sit down and power off its motors."
)
speak("Stopping HCM", self.blackboard.speak_publisher, priority=50)
# now wait for it finishing the shutdown procedure
while not self.blackboard.current_state == RobotControlState.HCM_OFF:
# we still have to update everything
self.blackboard.current_time = self.node.get_clock().now()
self.dsd.update()
self.hcm_state_publisher.publish(self.blackboard.current_state)
self.node.get_clock().sleep_for(Duration(seconds=0.01))
class ROS2TopicManager(object):
"""
"""
##
# @if jp
# @brief コンストラクタ
#
# コンストラクタ
#
# @param self
#
# @else
# @brief Constructor
#
# @param self
#
# @endif
def __init__(self):
self._thread = None
self._loop = True
#mgr = OpenRTM_aist.Manager.instance()
# mgr.addManagerActionListener(ManagerActionListener(self))
#self._rtcout = mgr.getLogbuf("ROS2TopicManager")
##
# @if jp
# @brief デストラクタ
#
#
# @param self
#
# @else
#
# @brief self
#
# @endif
def __del__(self):
pass
##
# @if jp
# @brief ROS2初期化
#
# @param self
# @param args rclpy.initの引数
#
# @else
#
# @brief
#
# @param self
# @param args
#
# @endif
def start(self, args=[]):
rclpy.init(args=args)
self._node = Node("openrtm")
def spin():
while self._loop:
rclpy.spin_once(self._node, timeout_sec=0.01)
self._thread = threading.Thread(target=spin)
self._thread.daemon = True
self._thread.start()
##
# @if jp
# @brief 終了処理
#
# @param self
#
# @else
#
# @brief
#
# @param self
#
# @endif
def shutdown(self):
if self._node:
self._loop = False
self._node.destroy_node()
# rclpy.try_shutdown()
# if self._thread:
# self._thread.join()
##
# @if jp
# @brief Publisherオブジェクト生成
#
# @param self
# @param msgtype メッセージ型
# @param topic トピック名
# @return Publisherオブジェクト
#
# @else
#
# @brief
#
# @param self
# @param msgtype
# @param topic
# @return
#
# @endif
def createPublisher(self, msgtype, topic):
global mutex
guard = OpenRTM_aist.ScopedLock(mutex)
if self._node:
return self._node.create_publisher(msgtype, topic)
return None
##
# @if jp
# @brief Subscriberオブジェクト生成
#
# @param self
# @param msgtype メッセージ型
# @param topic トピック名
# @param listener コールバック関数
# @return Subscriberオブジェクト
#
# @else
#
# @brief
#
# @param self
# @param msgtype
# @param topic
# @param listener
# @return
#
# @endif
def createSubscriber(self, msgtype, topic, listener):
global mutex
guard = OpenRTM_aist.ScopedLock(mutex)
if self._node:
return self._node.create_subscription(msgtype, topic, listener)
return None
def deletePublisher(self, pub):
pass
def deleteSubscriber(self, sub):
pass
##
# @if jp
# @brief インスタンス取得
#
# @return インスタンス
#
# @else
#
# @brief
#
# @return インスタンス
#
# @endif
def instance(args=[]):
global manager
global mutex
guard = OpenRTM_aist.ScopedLock(mutex)
if manager is None:
manager = ROS2TopicManager()
manager.start(args)
return manager
instance = staticmethod(instance)
##
# @if jp
# @brief ROS2TopicManagerを初期化している場合に終了処理を呼び出す
#
#
# @else
#
# @brief
#
#
# @endif
def shutdown_global():
global manager
global mutex
guard = OpenRTM_aist.ScopedLock(mutex)
if manager is not None:
manager.shutdown()
manager = None
shutdown_global = staticmethod(shutdown_global)
from rclpy.node import Node
from std_msgs.msg import Float32, ByteMultiArray
import struct
def get_hit(msg):
hits = msg.data
for i, hit in enumerate(hits):
hit = struct.unpack('b', hit)[0]
if hit:
print(f'Contact on sensor: {i}')
ros.init()
node = Node('Circle')
left_pub = node.create_publisher(Float32, 'robot0/left_wheel')
right_pub = node.create_publisher(Float32, 'robot0/right_wheel')
contact_sub = node.create_subscription(ByteMultiArray, 'robot0/touches', get_hit)
msg = Float32()
msg.data = 5.0
left_pub.publish(msg)
msg.data = 10.0
right_pub.publish(msg)
try:
ros.spin(node)
except KeyboardInterrupt:
import rclpy
from rclpy.node import Node
from std_msgs.msg import Int16
rclpy.init()
node = Node("talker")
pub = node.create_publisher(Int16, "countup", 10)
n = 0
def cb():
global n
msg = Int16()
msg.data = n
pub.publish(msg)
n += 1
timer = node.create_timer(0.5, cb)
rclpy.spin(node)
class GSPNExecutionROS(object):
def __init__(self, gspn, place_to_client_mapping, policy, project_path,
initial_place, robot_id):
'''
:param gspn: a previously created gspn
:param place_to_client_mapping: dictionary where key is the place and the value is the function
:param policy: Policy object
:param project_path: string with project path
:param initial_place: string with the name of the robot's initial place
self.__token_positions is a list with the places where each token is ['p1', 'p2', 'p2'] means that token 1 is on p1, token 2
is on p2 and token 3 is on p2;
self.__number_of_tokens is the current number of tokens;
self.__action_clients is a list with all the action clients;
self.__client_node is the node where the clients will be connected to.
self.__current_place indicates where the robot is
self.__action_client is the action client of the robot
self.__robot_id is the id of the robot
'''
self.__gspn = gspn
self.__token_positions = []
self.__place_to_client_mapping = place_to_client_mapping
self.__policy = policy
self.__project_path = project_path
self.__number_of_tokens = 0
# should be removed after correcting many to many/many to 1/1 to many
self.__action_clients = []
# --
self.__client_node = 0
self.__client_node_subscriber = 0
self.__current_place = initial_place
self.__action_client = 0
self.__robot_id = robot_id
def get_path(self):
return self.__project_path
def get_policy(self):
return self.__policy
def convert_to_tuple(self, marking, order):
'''
:param marking: dictionary with key= places; value= number of tokens in place
:param order: tuple of strings with the order of the marking on the policy
:return: tuple with the marking
'''
marking_list = []
for element in order:
for key in marking:
if element == key:
marking_list.append(marking[key])
return tuple(marking_list)
def get_transitions(self, marking, policy_dictionary):
'''
:param marking: tuple with the current marking (should already be ordered)
:param policy_dictionary: dictionary where key=Transition name; value=probability of transition
:return: transition dictionary if marking is in policy_dictionary; False otherwise
'''
for mark in policy_dictionary:
if marking == mark:
return policy_dictionary[mark]
return False
def translate_arcs_to_marking(self, arcs):
'''
: param arcs: set of connected add_arcs
: return: dictionary where key is the place and the value is always 1
This function essentially is used to determine which places are connected to the next transition.
The purpose of this is to later on compare it with the current marking.
'''
translation = {}
for i in arcs[0]:
place = self.__gspn.index_to_places[i]
translation[place] = 1
return translation
'''
Callback functions section:
In this section, we included every callback function that is being used both by our Action Clients and
our publishers/subscribers:
- topic_listener_callback;
- topic_talker_callback;
- action_get_result_callback;
- action_goal_response_callback;
- action_feedback_callback;
- action_send_goal
'''
def topic_listener_callback(self, msg):
if msg.robot_id != self.__robot_id:
self.__client_node.get_logger().info(
'I heard Robot %s firing %s' % (msg.robot_id, msg.transition))
print("BEFORE", self.__gspn.get_current_marking())
self.fire_execution(msg.transition)
print("AFTER", self.__gspn.get_current_marking())
else:
self.__client_node.get_logger().info('I heard myself firing %s' %
msg.transition)
def topic_talker_callback(self, fired_transition):
# the topic message is composed by four elements:
# - fired transition;
# - current marking;
# - robot_id;
# - timestamp.
msg = GSPNFiringData()
current_time = self.__client_node.get_clock().now()
msg.transition = str(fired_transition)
msg.marking = str(self.__gspn.get_current_marking())
msg.robot_id = self.__robot_id
print("current time ", current_time)
msg.timestamp = str(current_time)
self.__client_node.publisher.publish(msg)
self.__client_node.get_logger().info('Robot %s firing %s' %
(msg.robot_id, msg.transition))
self.__client_node.i += 1
def action_get_result_callback(self, future):
result = future.result().result
status = future.result().status
if status == GoalStatus.STATUS_SUCCEEDED:
print(self.__action_client._action_name +
': Goal succeeded! Result: {0}'.format(result.transition))
bool_output_arcs = self.check_output_arcs(self.__current_place)
if bool_output_arcs:
print("The place has output arcs.")
print("BEFORE", self.__gspn.get_current_marking())
if result.transition == 'None':
print("Immediate transition")
imm_transition_to_fire = self.get_policy_transition()
if imm_transition_to_fire == False:
print("The policy does not include this case: ",
self.__gspn.get_current_marking())
self.__client_node.destroy_client(self.__action_client)
self.__client_node.destroy_node()
return
else:
self.fire_execution(imm_transition_to_fire)
self.topic_talker_callback(imm_transition_to_fire)
else:
print("exponential transition")
print(result.transition)
self.fire_execution(result.transition)
self.topic_talker_callback(result.transition)
print("AFTER", self.__gspn.get_current_marking())
action_type = self.__place_to_client_mapping[
self.__current_place][0]
server_name = self.__place_to_client_mapping[
self.__current_place][1]
self.__client_node.destroy_client(self.__action_client)
self.__action_client = rclpy.action.ActionClient(
self.__client_node, action_type, server_name)
current_place = self.__current_place
self.action_send_goal(current_place, action_type, server_name)
else:
print("The place has no output arcs.")
self.__client_node.destroy_client(self.__action_client)
self.__client_node.destroy_node()
else:
print(self.__action_client._action_name +
': Goal failed with status: {0}'.format(status))
def action_goal_response_callback(self, future):
goal_handle = future.result()
if not goal_handle.accepted:
print('Goal rejected :( ' + self.__action_client._action_name)
return
print('Goal accepted :) ' + self.__action_client._action_name)
self.__action_client._get_result_future = goal_handle.get_result_async(
)
self.__action_client._get_result_future.add_done_callback(
self.action_get_result_callback)
def action_feedback_callback(self, feedback):
print('Received feedback: {0}'.format(feedback.feedback.time_passed))
def action_send_goal(self, current_place, action_type, server_name):
print('Waiting for action server ' + server_name)
self.__action_client.wait_for_server()
goal_msg = action_type.Goal()
goal_msg.place = current_place
print('Sending goal request to ' + server_name)
self.__action_client._send_goal_future = self.__action_client.send_goal_async(
goal_msg, feedback_callback=self.action_feedback_callback)
self.__action_client._send_goal_future.add_done_callback(
self.action_goal_response_callback)
'''
GSPN Execution core functions section:
In this section, we included the four main functions that are responsible for the execution of our
GSPNs:
- fire execution;
- get_immediate_transition_result;
- check_output_arcs;
- ros_gspn_execution.
'''
def fire_execution(self, transition):
'''
Fires the selected transition.
:param transition: string with transition that should be fired
'''
arcs = self.__gspn.get_connected_arcs(transition, 'transition')
index = self.__gspn.transitions_to_index[transition]
marking = self.__gspn.get_current_marking()
# 1 to 1
if len(arcs[0]) == 1 and len(arcs[1][index]) == 1:
new_place = self.__gspn.index_to_places[arcs[1][index][0]]
self.__gspn.fire_transition(transition)
self.__current_place = new_place
# 1 to many
elif len(arcs[0]) == 1 and len(arcs[1][index]) > 1:
i = 0
for i in range(len(arcs[1][index])):
if i == 0:
new_place = self.__gspn.index_to_places[arcs[1][index][i]]
# self.__token_positions[token_id] = new_place
else:
new_place = self.__gspn.index_to_places[arcs[1][index][i]]
self.__token_positions.append(new_place)
self.__number_of_tokens = self.__number_of_tokens + 1
self.__action_clients.append(
client.MinimalActionClient("provisional",
node=self.__client_node,
server_name="provisional"))
self.__gspn.fire_transition(transition)
# many to 1
elif len(arcs[0]) > 1 and len(arcs[1][index]) == 1:
translation_marking = self.translate_arcs_to_marking(arcs)
check_flag = True
# We go through the marking and check it
for el in translation_marking:
if marking[el] < translation_marking[el]:
check_flag = False
# We go through the states and see if all of them are 'Waiting'
number_of_waiting = 0
for place in translation_marking:
for pos_index in range(len(self.__token_positions)):
if self.__token_positions[pos_index] == place:
if self.__action_clients[pos_index].get_state(
) == 'Waiting':
number_of_waiting = number_of_waiting + 1
break
# -1 porque só precisas de ter x-1 a espera. quando o numero x
# aparece, podes disparar
if number_of_waiting == len(translation_marking) - 1:
check_flag = True
else:
check_flag = False
if check_flag:
new_place = self.__gspn.index_to_places[arcs[1][index][0]]
# old_place = self.__token_positions[token_id]
# self.__token_positions[token_id] = new_place
self.__gspn.fire_transition(transition)
for place_index in arcs[0]:
place_with_token_to_delete = self.__gspn.index_to_places[
place_index]
if place_with_token_to_delete != old_place:
for j in range(len(self.__token_positions)):
if place_with_token_to_delete == self.__token_positions[
j]:
index_to_del = j
self.__token_positions[index_to_del] = "null"
self.__action_clients[index_to_del].set_state(
"VOID")
break
else:
print("many to one")
# self.__action_clients[token_id].set_state("Waiting")
# many to many
elif len(arcs[0]) > 1 and len(arcs[1][index]) > 1:
translation_marking = self.translate_arcs_to_marking(arcs)
check_flag = True
for el in translation_marking:
if marking[el] < translation_marking[el]:
check_flag = False
if check_flag:
# Create tokens on next places
i = 0
for i in range(len(arcs[1][index])):
if i == 0:
new_place = self.__gspn.index_to_places[arcs[1][index]
[i]]
# self.__token_positions[token_id] = new_place
else:
new_place = self.__gspn.index_to_places[arcs[1][index]
[i]]
self.__token_positions.append(new_place)
self.__number_of_tokens = self.__number_of_tokens + 1
self.__action_clients.append(
client.MinimalActionClient(
"provisional",
node=self.__client_node,
server_name="provisional"))
self.__gspn.fire_transition(transition)
# Delete tokens from previous places
for place_index in arcs[0]:
place_with_token_to_delete = self.__gspn.index_to_places[
place_index]
for j in range(len(self.__token_positions)):
if place_with_token_to_delete == self.__token_positions[
j]:
index_to_del = j
self.__token_positions[index_to_del] = "null"
self.__action_clients[index_to_del].set_state(
"VOID")
break
else:
print("many to many")
# self.__action_clients[token_id].set_state("Waiting")
def get_policy_transition(self):
execution_policy = self.get_policy()
current_marking = self.__gspn.get_current_marking()
order = execution_policy.get_places_tuple()
marking_tuple = self.convert_to_tuple(current_marking, order)
pol_dict = execution_policy.get_policy_dictionary()
transition_dictionary = self.get_transitions(marking_tuple, pol_dict)
if transition_dictionary == False:
return False
transition_list = []
probability_list = []
for transition in transition_dictionary:
transition_list.append(transition)
probability_list.append(transition_dictionary[transition])
transition_to_fire = np.random.choice(transition_list, 1, False,
probability_list)[0]
print("TRANSITION TO FIRE", transition_to_fire)
return transition_to_fire
def check_output_arcs(self, place):
arcs = self.__gspn.get_connected_arcs(place, 'place')
arcs_out = arcs[1]
if len(arcs_out) >= 1:
return True
else:
return False
def ros_gspn_execution(self):
'''
Setup of the execution:
1- project path;
2- number of initial tokens;
3- token_positions list;
4- action servers;
5- initial action clients.
'''
# Setup project path
path_name = self.get_path()
self.__project_path = os.path.join(path_name)
sys.path.append(self.__project_path)
# Setup number of (initial) tokens
self.__number_of_tokens = self.__gspn.get_number_of_tokens()
# Setup token_positions list
marking = self.__gspn.get_current_marking()
for place in marking:
j = 0
while j != marking[place]:
self.__token_positions.append(place)
j = j + 1
#Setup action servers
## TODO:
rclpy.init()
# Setup client node with publisher and subscriber
node_name = "executor_" + str(self.__robot_id)
self.__client_node = Node(node_name,
namespace="robot_" + str(self.__robot_id))
self.__client_node.publisher = self.__client_node.create_publisher(
GSPNFiringData, '/TRANSITIONS_FIRED', 10)
self.__client_node.subscription = self.__client_node.create_subscription(
GSPNFiringData, '/TRANSITIONS_FIRED', self.topic_listener_callback,
10)
self.__client_node.subscription # prevent unused variable warning
self.__client_node.i = 0
action_type = self.__place_to_client_mapping[self.__current_place][0]
server_name = self.__place_to_client_mapping[self.__current_place][1]
self.__action_client = rclpy.action.ActionClient(
self.__client_node, action_type, server_name)
current_place = self.__current_place
self.action_send_goal(current_place, action_type, server_name)
rclpy.spin(self.__client_node)
def main():
rclpy.init(args=None)
node = Node('system_monitor')
pub = node.create_publisher(WorkloadMsg, 'system_workload', 1)
diagnostic_pub = node.create_publisher(DiagnosticArray, 'diagnostics', 1)
hostname = socket.gethostname()
diag_array = DiagnosticArray()
diag_cpu = DiagnosticStatus()
# start all names with "SYSTEM" for diagnostic analysesr
diag_cpu.name = "SYSTEMCPU"
diag_cpu.hardware_id = "CPU"
diag_mem = DiagnosticStatus()
diag_mem.name = "SYSTEMMemory"
diag_cpu.hardware_id = "Memory"
node.declare_parameter('update_frequency', 10.0)
node.declare_parameter('do_memory', True)
node.declare_parameter('do_cpu', True)
node.declare_parameter('cpu_load_percentage', 80.0)
node.declare_parameter('memoroy_load_percentage', 80.0)
node.declare_parameter('network_rate_received_errors', 10.0)
node.declare_parameter('network_rate_send_errors', 10.0)
rate = node.get_parameter('update_frequency').get_parameter_value().double_value
do_memory = node.get_parameter('do_memory').get_parameter_value().bool_value
do_cpu = node.get_parameter('do_cpu').get_parameter_value().bool_value
cpu_load_percentage = node.get_parameter('cpu_load_percentage').get_parameter_value().double_value
memoroy_load_percentage = node.get_parameter('memoroy_load_percentage').get_parameter_value().double_value
network_rate_received_errors = node.get_parameter(
'network_rate_received_errors').get_parameter_value().double_value
network_rate_send_errors = node.get_parameter('network_rate_send_errors').get_parameter_value().double_value
while rclpy.ok():
last_send_time = time.time()
running_processes, cpu_usages, overall_usage_percentage = cpus.collect_all() if do_cpu else (
-1, [], 0)
memory_available, memory_used, memory_total = memory.collect_all() if do_memory else (-1, -1, -1)
interfaces = network_interfaces.collect_all(node.get_clock())
msg = WorkloadMsg(
hostname=hostname,
cpus=cpu_usages,
running_processes=running_processes,
cpu_usage_overall=overall_usage_percentage,
memory_available=memory_available,
memory_used=memory_used,
memory_total=memory_total,
filesystems=[],
network_interfaces=interfaces
)
pub.publish(msg)
diag_array.status = []
# publish diagnostic message to show this in the rqt diagnostic viewer
diag_cpu.message = str(overall_usage_percentage) + "%"
if overall_usage_percentage >= cpu_load_percentage:
diag_cpu.level = DiagnosticStatus.WARN
else:
diag_cpu.level = DiagnosticStatus.OK
diag_array.status.append(diag_cpu)
memory_usage = round((memory_used / memory_total) * 100, 2)
diag_mem.message = str(memory_usage) + "%"
if memory_usage >= memoroy_load_percentage:
diag_mem.level = DiagnosticStatus.WARN
else:
diag_mem.level = DiagnosticStatus.OK
diag_array.status.append(diag_mem)
for interface in interfaces:
diag_net = DiagnosticStatus()
diag_net.name = "SYSTEM" + interface.name
diag_net.hardware_id = interface.name
if interface.rate_received_packets_errors >= network_rate_received_errors \
or interface.rate_sent_packets_errors >= network_rate_send_errors:
diag_net.level = DiagnosticStatus.WARN
else:
diag_net.level = DiagnosticStatus.OK
diag_array.status.append(diag_net)
diag_array.header.stamp = node.get_clock().now().to_msg()
diagnostic_pub.publish(diag_array)
# sleep to have correct rate. we dont use ROS time since we are interested in system things
dt = time.time() - last_send_time
time.sleep(max(0, (1 / rate) - dt))
// SPDX-License-Identifier: BSD-3.0
/*
Copyright (C) 2020 Yuka Matsuura and Ryuichi Ueda. All right reserved.
*/
import rclpy
from rclpy.node import Node
from std_msgs.msg import Float32
rclpy.init()
node= Node("talker") #ノード作成
pub = node.create_publisher(Float32, "BMI", 10) #パブリッシャ作成
def BMI():
global weight
global height
msg = Float32()
msg2 = Float32()
weight = float(input("How much do yo weight?(Kg)"))
height = float(input("How tall are you?(m)"))
msg.data = weight
msg2.data = height
pub.publish(msg)
pub.publish(msg2)
node.create_timer(0.5, BMI) #タイマー設定
rclpy.spin(node) #実行
def main():
args = sys.argv
joyname = ""
if len(args) > 1:
joyname = args[1]
else:
# Iterate over the joystick devices.
print("Available Joysticks:")
for fn in os.listdir('/dev/input'):
if fn.startswith('js'):
print('\t%s' % (fn))
sys.exit()
# We'll store the states here.
axis_states = {}
button_states = {}
axis_map = []
button_map = []
# Open the joystick device.
fn = '/dev/input/' + joyname
print('Opening %s...' % fn)
jsdev = open(fn, 'rb')
# Get the device name.
#buf = bytearray(63)
buf = array.array('b', [0] * 64)
ioctl(jsdev, 0x80006a13 + (0x10000 * len(buf)), buf) # JSIOCGNAME(len)
js_name = buf.tostring()
print('Device name: %s' % js_name)
# Get number of axes and buttons.
buf = array.array('B', [0])
ioctl(jsdev, 0x80016a11, buf) # JSIOCGAXES
num_axes = buf[0]
buf = array.array('B', [0])
ioctl(jsdev, 0x80016a12, buf) # JSIOCGBUTTONS
num_buttons = buf[0]
# Get the axis map.
buf = array.array('B', [0] * 0x40)
ioctl(jsdev, 0x80406a32, buf) # JSIOCGAXMAP
for axis in buf[:num_axes]:
axis_name = axis_names.get(axis, 'unknown(0x%02x)' % axis)
axis_map.append(axis_name)
axis_states[axis_name] = 0.0
# Get the button map.
buf = array.array('H', [0] * 200)
ioctl(jsdev, 0x80406a34, buf) # JSIOCGBTNMAP
for btn in buf[:num_buttons]:
btn_name = button_names.get(btn, 'unknown(0x%03x)' % btn)
button_map.append(btn_name)
button_states[btn_name] = 0
print('%d axes found: %s' % (num_axes, ', '.join(axis_map)))
print('%d buttons found: %s' % (num_buttons, ', '.join(button_map)))
# Spin child process to read the joystick
# with blocking reads
events = Queue()
t = Process(target=readJoystick, args=(jsdev, events))
t.start()
rclpy.init()
node = Node("joystick_" + joyname)
pubjoy = node.create_publisher(Joy, joyname + '/joystick')
simTime = SimTimer(False, "veranda/timestamp", node)
# Tick time at 10 hz
dt = 0.1
def cb():
processJoystick(events, button_states, axis_states, button_map,
axis_map)
msg = formatJoystickMessage(button_states, axis_states)
pubjoy.publish(msg)
simTime.create_timer(dt, cb)
rclpy.spin(node)
node.destroy_node()
rclpy.shutdown()
t.terminate()
