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