def subscribe_and_spin( self, node: Node, topic_name: str, message_type: MsgType, qos_profile: QoSProfile, no_report_lost_messages: bool, raw: bool ) -> Optional[str]: """Initialize a node with a single subscription and spin.""" event_callbacks = None if not no_report_lost_messages: event_callbacks = SubscriptionEventCallbacks( message_lost=_message_lost_event_callback) try: node.create_subscription( message_type, topic_name, self._subscriber_callback, qos_profile, event_callbacks=event_callbacks, raw=raw) except UnsupportedEventTypeError: assert not no_report_lost_messages node.create_subscription( message_type, topic_name, self._subscriber_callback, qos_profile, event_callbacks=None, raw=raw) rclpy.spin(node)
def subscriber(node: Node, topic_name: str, message_type: MsgType, callback: Callable[[MsgType], Any], qos_profile: QoSProfile) -> Optional[str]: """Initialize a node with a single subscription and spin.""" if message_type is None: topic_names_and_types = get_topic_names_and_types( node=node, include_hidden_topics=True) try: expanded_name = expand_topic_name(topic_name, node.get_name(), node.get_namespace()) except ValueError as e: raise RuntimeError(e) try: validate_full_topic_name(expanded_name) except rclpy.exceptions.InvalidTopicNameException as e: raise RuntimeError(e) for n, t in topic_names_and_types: if n == expanded_name: if len(t) > 1: raise RuntimeError( "Cannot echo topic '%s', as it contains more than one type: [%s]" % (topic_name, ', '.join(t))) message_type = t[0] break else: raise RuntimeError( 'Could not determine the type for the passed topic') msg_module = get_message(message_type) node.create_subscription(msg_module, topic_name, callback, qos_profile) rclpy.spin(node)
def subscriber(node: Node, topic_name: str, message_type: MsgType, callback: Callable[[MsgType], Any], qos_profile: QoSProfile) -> Optional[str]: """Initialize a node with a single subscription and spin.""" node.create_subscription(message_type, topic_name, callback, qos_profile) rclpy.spin(node)
def __init__(self, node: Node) -> None: self.node = node self.waypoints = [] self.balls = [] 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(Point, "/target", qos_profile_services_default) node.create_subscription(Float32MultiArray, "/balls_coords", self.__balls_callback, qos_profile_sensor_data) node.create_subscription(Float64MultiArray, "/waypoints", self.__wp_callback, qos_profile_sensor_data)
def subscriber(node: Node, topic_name: str, message_type: MsgType, callback: Callable[[MsgType], Any], qos_profile: QoSProfile, report_lost_messages: bool, future=None, timeout=None) -> Optional[str]: """Initialize a node with a single subscription and spin.""" event_callbacks = None if report_lost_messages: event_callbacks = SubscriptionEventCallbacks( message_lost=message_lost_event_callback) try: node.create_subscription(message_type, topic_name, callback, qos_profile, event_callbacks=event_callbacks) except UnsupportedEventTypeError: assert report_lost_messages print(f"The rmw implementation '{get_rmw_implementation_identifier()}'" ' does not support reporting lost messages') rclpy.spin_until_future_complete(node, future, timeout) if future is not None: if not future.done(): node.get_logger().info('Timeout occured')
def __init__(self, buffer: Buffer, node: Node, *, spin_thread: bool = False, qos: Optional[Union[QoSProfile, int]] = None, static_qos: Optional[Union[QoSProfile, int]] = None) -> None: """ Constructor. :param buffer: The buffer to propagate changes to when tf info updates. :param node: The ROS2 node. :param spin_thread: Whether to create a dedidcated thread to spin this node. :param qos: A QoSProfile or a history depth to apply to subscribers. :param static_qos: A QoSProfile or a history depth to apply to tf_static subscribers. """ if qos is None: qos = QoSProfile( depth=100, durability=DurabilityPolicy.VOLATILE, history=HistoryPolicy.KEEP_LAST, ) if static_qos is None: static_qos = QoSProfile( depth=100, durability=DurabilityPolicy.TRANSIENT_LOCAL, history=HistoryPolicy.KEEP_LAST, ) self.buffer = buffer self.node = node # Default callback group is mutually exclusive, which would prevent waiting for transforms # from another callback in the same group. self.group = ReentrantCallbackGroup() self.tf_sub = node.create_subscription(TFMessage, '/tf', self.callback, qos, callback_group=self.group) self.tf_static_sub = node.create_subscription( TFMessage, '/tf_static', self.static_callback, static_qos, callback_group=self.group) if spin_thread: self.executor = SingleThreadedExecutor() def run_func(): self.executor.add_node(self.node) self.executor.spin() self.executor.remove_node(self.node) self.dedicated_listener_thread = Thread(target=run_func) self.dedicated_listener_thread.start()
def __init__( self, node: Node, topic: str, message_type: type, updater: Updater, frequency: float, ): self._frequency_params = FrequencyStatusParam({"min": frequency}) self._updater = updater self._diagnostics = {} node.create_subscription(message_type, topic, self._cb, qos_profile=10)
def Robotiq2FGripperStatusListener(): """Initialize the node and subscribe to the Robotiq2FGripperRobotInput topic.""" rclpy.init() node = Node('Robotiq2FGripperStatusListener') node.create_subscription(inputMsg, "Robotiq2FGripperRobotInput", printStatus, 1) try: rclpy.spin(node) except KeyboardInterrupt: pass node.destroy_node() rclpy.shutdown()
def wait_for_messages(self, themes): # tests_params = {<name>: {'callback', 'topic', 'msg_type', 'internal_params'}} self.func_data = dict([[theme_name, {}] for theme_name in themes]) node = Node('wait_for_messages') for theme_name in themes: theme = self.themes[theme_name] node.get_logger().info('Subscribing %s on topic: %s' % (theme_name, theme['topic'])) self.func_data[theme_name]['sub'] = node.create_subscription( theme['msg_type'], theme['topic'], theme['callback'](theme_name), qos.qos_profile_sensor_data) self.tfBuffer = tf2_ros.Buffer() self.tf_listener = tf2_ros.TransformListener(self.tfBuffer, node) self.prev_time = time.time() break_timeout = False while not break_timeout: rclpy.spin_once(node, timeout_sec=1) if self.timeout > 0 and time.time() - self.prev_time > self.timeout: break_timeout = True self.unregister_all(node, self.func_data) node.destroy_node() return self.func_data
def __init__(self, node: Node, topic_name: str, message_type: str, expected_values: Dict[str, str], callback: Callable[[bool, Dict[str, str], Dict[str, str]], None] = default_callback): self.__callback = callback self.__expected_values = expected_values self.__message_type = message_type self.__node = node latching_qos = QoSProfile(depth=1, durability=QoSDurabilityPolicy. RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL) node.get_logger().info("Subscribing to \"{}\" of type \"{}\"".format( topic_name, message_type)) node.get_logger().info( "Expecting to recieve message with value \"{}\"".format( str(expected_values))) self.__sub = node.create_subscription(get_message(message_type), topic_name, self.__sub_callback, qos_profile=latching_qos)
def __init__(self, node: Node, joint_name: str, topic: str, msg_type: type): self.node = node self.joint_name = joint_name node.create_subscription(msg_type, topic, self._cb, qos_profile=10) ( self._default_threshold, self._thresholds_warning, self._thresholds_non_fatal, self._thresholds_fatal, ) = self._get_temperature_thresholds() self._timestamp = None self._temperature = None
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 main(args=None): topic = '/ifm3d/camera/unit_vectors' topic_type = Image rclpy.init(args=args) qos_profile = QoSProfile( depth=1, reliability=QoSReliabilityPolicy.RELIABLE, durability=QoSDurabilityPolicy.TRANSIENT_LOCAL ) node = Node('uvec_listener') sub = node.create_subscription( topic_type, topic, lambda msg: print(msg), qos_profile) try: rclpy.spin(node) except KeyboardInterrupt: node.destroy_node() rclpy.shutdown() finally: return 0
def wait_for_message(self, params, msg_type=msg_Image): topic = params['topic'] print ('connect to ROS with name: %s' % self.node_name) rclpy.init() node = Node(self.node_name) out_filename = params.get('filename', None) if (out_filename): self.fout = open(out_filename, 'w') if msg_type is msg_Imu: col_w = 20 print ('Writing to file: %s' % out_filename) columns = ['frame_number', 'frame_time(sec)', 'accel.x', 'accel.y', 'accel.z', 'gyro.x', 'gyro.y', 'gyro.z'] line = ('{:<%d}'*len(columns) % (col_w, col_w, col_w, col_w, col_w, col_w, col_w, col_w)).format(*columns) + '\n' sys.stdout.write(line) self.fout.write(line) node.get_logger().info('Subscribing on topic: %s' % topic) sub = node.create_subscription(msg_type, topic, self.callback, qos.qos_profile_sensor_data) self.prev_time = time.time() break_timeout = False while not any([(not rclpy.ok()), break_timeout, self.result]): rclpy.spin_once(node) if self.timeout > 0 and time.time() - self.prev_time > self.timeout: break_timeout = True node.destroy_subscription(sub) return self.result
def make_mock_subscription(namespace, topic_name, cli_args=None): node = Node('node_name', namespace=namespace, cli_args=cli_args) return node.create_subscription( msg_type=Empty, topic=topic_name, callback=lambda _: None, qos_profile=10, )
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 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 test_get_subscription_topic_name_after_remapping(topic_name, namespace, cli_args, expected): node = Node('node_name', namespace=namespace, cli_args=cli_args) sub = node.create_subscription( msg_type=Empty, topic=topic_name, callback=lambda _: None, qos_profile=10, ) assert sub.topic_name == expected
def init(self, node: Node, yellow_: bool) -> None: self.node_ = node self.yellow_ = yellow_ # Robots self.p_allies_ = node.create_subscription(Robots, 'allies', self.update_allies, 10) self.p_opponents_ = node.create_subscription(Robots, 'opponents', self.update_opponents, 10) # Ball self.p_ball_ = node.create_subscription(Ball, 'ball', self.update_ball, 10) # GameController self.p_gc_ = node.create_subscription(Referee, 'gc', self.update_gc, 10)
def main(): args = sys.argv if len(args) != 2: print("Usage: joystick_listener {topic}") return channel = str(args[1]) print("Listening on topic \'" + channel + "\'") rclpy.init(args=args) node = Node("joystick_snooper") node.create_subscription(Joy, channel, joystick_callback) rclpy.spin(node) node.destroy_node() rclpy.shutdown()
def main(): args = sys.argv if len(args) != 2: print("Usage: lidar_listener topic") return channel = str(args[1]) print("Listening on topic \'" + channel + "\'") rclpy.init(args=args) node = Node("lidar_snooper") node.create_subscription(LaserScan, channel, lidar_callback) rclpy.spin(node) node.destroy_node() rclpy.shutdown()
def __init__(self, node: Node, topic: str, msg_type: type): node.create_subscription(msg_type, topic, self.cb, qos_profile=10) # callback variables self._timestamp: Optional[Time] = None self._joint_names: Optional[List[str]] = None self._position: Optional[List[float]] = None self._velocity: Optional[List[float]] = None self._effort: Optional[List[float]] = None # robot properties self._lower_soft_limits = {} self._upper_soft_limits = {} self._velocity_limits = {} self._effort_limits = {} robot = get_robot_urdf_from_service(node) for joint in robot.joints: self.set_joint_limits(joint)
def subscriber(node: Node, topic_name: str, message_type: MsgType, callback: Callable[[MsgType], Any], qos_profile: QoSProfile, report_lost_messages: bool, raw: bool) -> Optional[str]: """Initialize a node with a single subscription and spin.""" event_callbacks = None if report_lost_messages: event_callbacks = SubscriptionEventCallbacks( message_lost=message_lost_event_callback) try: node.create_subscription(message_type, topic_name, callback, qos_profile, event_callbacks=event_callbacks, raw=raw) except UnsupportedEventTypeError: assert report_lost_messages print(f"The rmw implementation '{get_rmw_implementation_identifier()}'" ' does not support reporting lost messages') rclpy.spin(node)
def init(node: Node): node.get_logger().info("Starting localization handler") global_params_dict = get_parameters_from_other_node( node, "parameter_blackboard", ["field_length", "team_id"]) global_params = [("field_length", global_params_dict["field_length"]), ("team_id", global_params_dict["team_id"])] node.declare_parameters(parameters=global_params, namespace="") blackboard = LocalizationBlackboard(node) dirname = get_package_share_directory("bitbots_localization_handler") node.get_logger().info(dirname) dsd = DSD(blackboard, "debug/dsd/localization", node) dsd.register_actions(os.path.join(dirname, 'actions')) dsd.register_decisions(os.path.join(dirname, 'decisions')) dsd.load_behavior(os.path.join(dirname, 'localization.dsd')) node.create_subscription(PoseWithCovarianceStamped, "pose_with_covariance", blackboard._callback_pose, 1) node.create_subscription(GameState, "gamestate", blackboard.gamestate.gamestate_callback, 1) node.create_subscription(RobotControlState, "robot_state", blackboard._callback_robot_control_state, 1) return dsd
def __init__(self, node: Node, updater: Updater): """Initializes an gait diagnostic which analyzes gait and subgait states. :type updater: diagnostic_updater.Updater """ self._goal_sub = node.create_subscription( topic="/march/gait_selection/current_gait", msg_type=CurrentGait, callback=self._cb_goal, qos_profile=10, ) self._gait_msg = None updater.add("Gait", self._diagnostics)
def __init__(self, node: Node, msg_type: MsgType, topic: str, callback: Callable, print_incoming_msg: bool = False) -> None: # Check if a ros2 node is provided. if (not isinstance(node, Node) or not issubclass(type(node), Node)): raise TypeError( "Input argument 'node' is not of type rclpy.node.Node!") # Check if topic already created. for operating_subscription in node.subscriptions: if topic[0] != "/": if "/" + topic is operating_subscription.topic: raise AttributeError( f"Subscription for topic, /{topic}, already created!") if topic is operating_subscription.topic: raise AttributeError( f"Subscription for topic, {topic}, already created!") self.node = node self.topic = topic self.msg_type = msg_type self.data = None self.__thread_state = False self.__widgets = { "start_btn": widgets.Button(description='Start'), "stop_btn": widgets.Button(description='Stop'), "out": widgets.Output(layout={'border': '1px solid gray'}), } if print_incoming_msg: self.__subscription = node.create_subscription( msg_type, topic, self.__print_msg(self.__data_msg(callback)), 10) else: self.__subscription = node.create_subscription( msg_type, topic, self.__data_msg(callback), 10)
def __init__(self, node: Node, updater: Updater, joint_names: List[str]): """Initialize an MotorController diagnostic which analyzes MotorController states. :type updater: diagnostic_updater.Updater """ self.node = node self._sub = node.create_subscription( msg_type=MotorControllerState, topic="/march/motor_controller_states", callback=self._cb, qos_profile=10, ) self._motor_controller_state = None for i, joint_name in enumerate(joint_names): updater.add(f"MotorController {joint_name}", self._diagnostic(i))
def main(): if len(sys.argv) < 2 or '--help' in sys.argv or '/?' in sys.argv: print ('USAGE:') print('echo_metadata.py <topic>') print('Demo for listening on given metadata topic.') print('App subscribes on given topic') print('App then prints metadata from messages') print('') print('Example: echo_metadata.py /camera/depth/metadata') print('') exit(-1) topic = sys.argv[1] rclpy.init() node = Node('metadata_tester') depth_sub = node.create_subscription(Metadata, topic, metadata_cb, qos.qos_profile_sensor_data) rclpy.spin(node)
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)