def __init__( self, node: Node, topic_and_expected_values_pairs: List[TopicAndValuesPair], eqaulity_type: EqualityType, callback: Callable[ [bool, Dict[str, bool], List[TopicAndValuesPair], EqualityType], None] = default_multi_message_callback): self.__equality_type = eqaulity_type self.__callback = callback self.__topic_and_expected_values_pairs = topic_and_expected_values_pairs node.get_logger().info("Setting up subscriptions") for topic_and_expected_values_pair in topic_and_expected_values_pairs: #Create MessageEqualityTester that store the result in __message_store when a new message #is received, and then perform the predicate check (which calls the callback) def cb(val: bool, actual_msg: Dict, expected_values: Dict, topic_and_expected_values_pair=topic_and_expected_values_pair ): #Store the value of that message self.__message_store[ topic_and_expected_values_pair.topic_name] = val node.get_logger().info("Message received ({}/{})".format( len(self.__message_store), len(topic_and_expected_values_pairs))) if (len(self.__message_store) == len( topic_and_expected_values_pairs)): node.get_logger().info( "All messages received, performing check") self.__perform_check() tester = MessageEqualityTester( node, topic_and_expected_values_pair.topic_name, topic_and_expected_values_pair.topic_type, topic_and_expected_values_pair.values, cb) self.__testers.append(tester) node.get_logger().info( "Waiting for at least one message on each topic...")
def __init__(self, node_name, *, enable_communication_interface: bool = True, **kwargs): """ Create a lifecycle node. See rclpy.lifecycle.LifecycleNodeMixin.__init__() and rclpy.node.Node() for the documentation of each parameter. """ Node.__init__(self, node_name, **kwargs) LifecycleNodeMixin.__init__( self, enable_communication_interface=enable_communication_interface)
def __init__(self, name, planning_rate: float, planner: AbstractPlannerImplementation): self.name = name Node.__init__(self, "planner_" + name) # Abstract implementation of planner (adapter) self.planner_adapter = planner # Basic I/O interface (input and refined trajectory) # Subscriber interface for trajectory planning self._refined_trajectory = self.create_publisher( Trajectory, '/'.join([self.name, 'refined_trajectory']), 1) self._input_trajectory = None # Planner timer cycle self._plan_timer = self.create_timer(planning_rate, self.cb_timer_planner) self.get_logger().info("Starting planner node {0}".format(self.name))
def __init__(self): # Initialise node try: rclpy.init() except: if not rclpy.ok(): import sys sys.exit("ROS 2 could not be initialised") Node.__init__(self, "octree_creator_test") self.__point_cloud_sub = self.create_subscription( PointCloud2, "rgbd_camera/points", self.point_cloud_callback, 1) self.octree_creator = OctreeCreator()
def timer_callback(self): """Timer Callback Function This method captures images and publishes required data in ros 2 topic. """ if self.capture.isOpened(): # reads image data ret, frame = self.capture.read() # processes image data and converts to ros 2 message msg = Image() msg.header.stamp = Node.get_clock(self).now().to_msg() msg.header.frame_id = 'ANI717' msg.height = np.shape(frame)[0] msg.width = np.shape(frame)[1] msg.encoding = "bgr8" msg.is_bigendian = False msg.step = np.shape(frame)[2] * np.shape(frame)[1] msg.data = np.array(frame).tobytes() # publishes message self.publisher_.publish(msg) self.get_logger().info('%d Images Published' % self.i) # image counter increment self.i += 1 return None
def main(): rclpy.init() node = Node('table_parameters', allow_undeclared_parameters=True, automatically_declare_parameters_from_overrides=True) N = node.get_parameter('N').value table = PositionTaskTable(N) table.gc.trigger() table.get_logger().info('Waiting for 10 seconds to let all nodes be ready') time.sleep(10) rclpy.spin(table) rclpy.shutdown()
def create_node(node_name: str, *, context: Context = None, cli_args: List[str] = None, namespace: str = None, use_global_arguments: bool = True, start_parameter_services: bool = True, initial_parameters: List[Parameter] = None) -> 'Node': """ Create an instance of :class:`.Node`. :param node_name: A name to give to the node. :param context: The context to associated with the node, or ``None`` for the default global context. :param cli_args: Command line arguments to be used by the node. :param namespace: The namespace prefix to apply to entities associated with the node (node name, topics, etc). :param use_global_arguments: ``False`` if the node should ignore process-wide command line arguments. :param start_parameter_services: ``False`` if the node should not create parameter services. :param initial_parameters: A list of :class:`.Parameter` to be set during node creation. :return: An instance of the newly created node. """ # imported locally to avoid loading extensions on module import from rclpy.node import Node return Node(node_name, context=context, cli_args=cli_args, namespace=namespace, use_global_arguments=use_global_arguments, start_parameter_services=start_parameter_services, initial_parameters=initial_parameters)
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 __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 main(): rclpy.init() node = Node('cartesian_compute') try: c = CartesianCompute(node) rclpy.spin(node) except KeyboardInterrupt: pass finally: node.destroy_node() rclpy.shutdown() return 0
def to_path_marker(self, node: Node, clear=False): """Return a `nav_msgs/Path` message with all waypoints in the set > *Input arguments* * `clear` (*type:* `bool`, *default:* `False`): Return an empty `nav_msgs/Path` message. > *Returns* `nav_msgs/Path` message """ path = Path() t = node.get_clock().now() path.header.stamp = t.to_msg() path.header.frame_id = self._inertial_frame_id if self.num_waypoints > 1 and not clear: for i in range(self.num_waypoints): wp = self.get_waypoint(i) pose = PoseStamped() pose.header.stamp = rclpy.time.Time(seconds=i) # TODO check pose.header.frame_id = self._inertial_frame_id pose.pose.position.x = wp.x pose.pose.position.y = wp.y pose.pose.position.z = wp.z path.poses.append(pose) return path
def create_node(node_name, *, context=None, cli_args=None, namespace=None, use_global_arguments=True, start_parameter_services=True, initial_parameters=None): """ Create an instance of :class:`rclpy.node.Node`. :param node_name: A unique name to give to this node. :param context: The context to be associated with, or None for the default global context. :param cli_args: A list of strings of command line args to be used only by this node. :param namespace: The namespace to which relative topic and service names will be prefixed. :param use_global_arguments: False if the node should ignore process-wide command line args. :param start_parameter_services: False if the node should not create parameter services. :param initial_parameters: A list of rclpy.parameter.Parameters to be set during node creation. :return: An instance of a node :rtype: :class:`rclpy.node.Node` """ # imported locally to avoid loading extensions on module import from rclpy.node import Node return Node(node_name, context=context, cli_args=cli_args, namespace=namespace, use_global_arguments=use_global_arguments, start_parameter_services=start_parameter_services, initial_parameters=initial_parameters)
def test_declare_qos_parameters(): node = Node('my_node') _declare_qos_parameters(Publisher, node, '/my_topic', QoSProfile(depth=10), QoSOverridingOptions.with_default_policies()) qos_overrides = node.get_parameters_by_prefix('qos_overrides') assert len(qos_overrides) == 3 expected_params = ( ('/my_topic.publisher.depth', 10), ('/my_topic.publisher.history', 'keep_last'), ('/my_topic.publisher.reliability', 'reliable'), ) for actual, expected in zip( sorted(qos_overrides.items(), key=lambda x: x[0]), expected_params): assert actual[0] == expected[0] # same param name assert actual[1].value == expected[1] # same param value
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 __init__(self, server_name, state, path): """Traverse the smach tree starting at root, and construct introspection proxies for getting and setting debug state.""" Node.__init__(self, server_name) # A list of introspection proxies self._proxies = [] # Store args self._server_name = server_name self._state = state self._path = path self._executor = SingleThreadedExecutor() self._executor.add_node(self) self._spinner = threading.Thread(target=self._executor.spin) self._spinner.start()
def __init__(self,origion_x=0.0,origion_y=0.0,resolution=0.1, width=50, height=50): super().__init__('map_pub') self.msg = OccupancyGrid() #self.pub = self.create_publisher(OccupancyGrid) #timer_period = 0.05 #self.timer = self.create_timer(timer_period, self.scan) self.time = Node.get_clock(self) self.time = self.get_clock().now().to_msg() self.orogion_x = origion_x self.origion_y = origion_y self.resolution = resolution self.width = width self.height = height self.grid = np.zeros((height, width)) self.time = Node.get_clock(self) self.time = self.get_clock().now().to_msg()
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).""" 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) 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 main(args=None): rclpy.init(args=args) node = Node("task_power_rune2019_client") cli = node.create_client(SetMode, "task_power_rune/set_mode") while not cli.wait_for_service(timeout_sec=1.0): node.get_logger().info('service not available, waiting again...') print(msg) old_attr = termios.tcgetattr(sys.stdin) tty.setcbreak(sys.stdin.fileno()) while rclpy.ok(): if select.select([sys.stdin], [], [], 0)[0] == [sys.stdin]: key=sys.stdin.read(1) print(key) if (key == 'q' or key == 'Q'): set_mode_req(cli,0x00) elif(key=='w' or key=='W'): set_mode_req(cli,0x01) elif(key=='e' or key=='E'): set_mode_req(cli,0x02) elif(key=='r' or key=='R'): set_mode_req(cli,0x03) elif(key=='a' or key=='A'): set_mode_req(cli,0x10) elif(key=='s' or key=='S'): set_mode_req(cli,0x11) termios.tcsetattr(sys.stdin, termios.TCSADRAIN, old_attr) node.destroy_node() rclpy.shutdown()
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 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 test_destruction_order(): context = rclpy.context.Context() rclpy.init(context=context) node1 = Node('test_destruction_order_node_1', context=context) node2 = Node('test_destruction_order_node_2', context=context) node3 = Node('test_destruction_order_node_3', context=context) node1.cyclic_ref = node1 node2.cyclic_ref = node2 node1.node3 = node3 node2.node3 = node3 node1.handle.context_handle = context.handle node2.handle.context_handle = context.handle
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.__init__(self, "pgcd_comp", allow_undeclared_parameters = True, automatically_declare_parameters_from_overrides = True) Interpreter.__init__(self, self.get_name()) TFUpdater.__init__(self) # program self.id = self.get_name() self.prog_path = self.get_parameter('program_location')._value + self.id + '.rosl' rclpy.logging._root_logger.log("PGCD creating " + self.id + " running " + self.prog_path, LoggingSeverity.INFO) # hardware module = importlib.import_module(self.get_parameter('object_module_name')._value) class_ = getattr(module, self.get_parameter('object_class_name')._value) self.robot = class_() # runtime self.tfBuffer = tf2_ros.Buffer() self.tf2_listener = tf2_ros.TransformListener(self.tfBuffer, self) self.tf2_setup(self.robot)
def __init__(self): super().__init__('pose_estimate') self.msg = geometry_msgs.msg.TransformStamped() self.pub = self.create_publisher(geometry_msgs.msg.TransformStamped, "/tf", 1) timer_period = 0.05 self.timer = self.create_timer(timer_period, self.scan) self.time = Node.get_clock(self) self.time = self.get_clock().now().to_msg()
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 run(self, node: Node, joy_state: sensor_msgs.msg.Joy) -> None: # The logic for responding to this joystick press is: # 1. Save off the current state of active. # 2. Update the current state of active based on buttons and axes. # 3. If this command is currently not active, return without publishing. # 4. If this is a msg_value, and the value of the previous active is the same as now, # debounce and return without publishing. # 5. In all other cases, publish. This means that this is a msg_value and the button # transitioned from 0 -> 1, or it means that this is an axis mapping and data should # continue to be published without debouncing. last_active = self.active self.update_active_from_buttons_and_axes(joy_state) if not self.active: return if self.msg_value is not None and last_active == self.active: return if self.msg_value is not None: # This is the case for a static message. msg = self.msg_value else: # This is the case to forward along mappings. msg = self.topic_type() for mapping, values in self.axis_mappings.items(): if 'axis' in values: if len(joy_state.axes) > values['axis']: val = joy_state.axes[values['axis']] * values.get('scale', 1.0) + \ values.get('offset', 0.0) else: node.get_logger().error('Joystick has only {} axes (indexed from 0),' 'but #{} was referenced in config.'.format( len(joy_state.axes), values['axis'])) val = 0.0 elif 'button' in values: if len(joy_state.buttons) > values['button']: val = joy_state.buttons[values['button']] * values.get('scale', 1.0) + \ values.get('offset', 0.0) else: node.get_logger().error('Joystick has only {} buttons (indexed from 0),' 'but #{} was referenced in config.'.format( len(joy_state.buttons), values['button'])) val = 0.0 elif 'value' in values: # Pass on the value as its Python-implicit type val = values.get('value') else: node.get_logger().error( 'No Supported axis_mappings type found in: {}'.format(mapping)) val = 0.0 set_member(msg, mapping, val) # If there is a stamp field, fill it with now(). if hasattr(msg, 'header'): msg.header.stamp = node.get_clock().now().to_msg() self.pub.publish(msg)
def __init__(self, topic: str, is_point_cloud: bool, use_sim_time: bool = True, node_name: str = 'drl_grasping_camera_sub'): try: rclpy.init() except: if not rclpy.ok(): import sys sys.exit("ROS 2 could not be initialised") Node.__init__(self, node_name) self.set_parameters([ Parameter('use_sim_time', type_=Parameter.Type.BOOL, value=use_sim_time) ]) # Prepare the subscriber if is_point_cloud: observation_type = PointCloud2 else: observation_type = Image self.__observation = observation_type() self.__observation_sub = self.create_subscription( msg_type=observation_type, topic=topic, callback=self.observation_callback, qos_profile=QoSProfile( durability=QoSDurabilityPolicy.SYSTEM_DEFAULT, reliability=QoSReliabilityPolicy.BEST_EFFORT, history=QoSHistoryPolicy.SYSTEM_DEFAULT)) self.__observation_mutex = Lock() self.__new_observation_available = False # Spin the node in a separate thread self._executor = SingleThreadedExecutor() self._executor.add_node(self) self._executor_thread = Thread(target=self._executor.spin, args=(), daemon=True) self._executor_thread.start()
def __init__(self, node:Node): self.node = node self.shut_down_request = False self.last_initialized = None self.initialized = False self.use_sim_time = self.node.get_parameter('use_sim_time').value # we only need tf in simulation. don't use it otherwise to safe performance if self.use_sim_time: self.tf_buffer = tf2.Buffer(cache_time=Duration(seconds=10)) self.tf_listener = tf2.TransformListener(self.tf_buffer, node) self.odom_frame = node.get_parameter('odom_frame').get_parameter_value().string_value self.base_footprint_frame = node.get_parameter('base_footprint_frame').get_parameter_value().string_value self.field_length = node.get_parameter('field_length').get_parameter_value().double_value # services self.reset_filter_proxy = node.create_client(ResetFilter, 'reset_localization') self.stop_filter_proxy = node.create_client(SetPaused, 'pause_localization') # Pose self.last_pose_update_time = None self.poseX = 0 self.poseY = 0 self.orientation = np.array([0, 0, 0, 1]) self.covariance = np.array([]) #GameState self.gamestate = GameStatusCapsule(node) #Robot Control State self.robot_control_state = None self.last_robot_control_state = None #Get up self.last_state_get_up = False #Picked up self.last_state_pickup = False #Last init action self.last_init_action_type = False self.last_init_odom_transform = None
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 = import_message_type(topic_name, 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) 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)
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)