def __init__(self): super().__init__('sonar2', namespace='') # Params #____________________________________________ # Publishers #____________________________________________ # toCpu2 # Qos profile qos_profile_toCpu2 = QoSProfile( history=QoSHistoryPolicy.KEEP_LAST, durability=QoSDurabilityPolicy.SYSTEM_DEFAULT, reliability=QoSReliabilityPolicy.SYSTEM_DEFAULT, depth=15) # Additional qos settings qos_profile_toCpu2.liveliness = QoSLivelinessPolicy.SYSTEM_DEFAULT qos_profile_toCpu2.deadline.sec = 0 qos_profile_toCpu2.deadline.nsec = 0 qos_profile_toCpu2.lifespan.sec = 0 qos_profile_toCpu2.lifespan.nsec = 0 qos_profile_toCpu2.liveliness_lease_duration.sec = 0 qos_profile_toCpu2.liveliness_lease_duration.nsec = 0 qos_profile_toCpu2.avoid_ros_namespace_conventions = False self.publisher_toCpu2 = self.create_publisher( Distance, 'topic/path/s2', qos_profile=qos_profile_toCpu2) self.timer_toCpu2 = self.create_timer(0.0, self.publisher_call_toCpu2) self.i = 0
def __init__(self): super().__init__('simple_api_server') api_req_qos_profile = QoSProfile(history=History.KEEP_LAST, depth=1, reliability=Reliability.RELIABLE, durability=Durability.TRANSIENT_LOCAL) self.task_api_req_pub = self.create_publisher(ApiRequest, '/task_api_requests', api_req_qos_profile) self.get_building_map_srv = self.create_client(GetBuildingMap, '/get_building_map') # to show robot states self.fleet_state_subscription = self.create_subscription( FleetState, 'fleet_states', self.fleet_state_cb, qos_profile=QoSProfile(depth=20)) self.fleet_states_dict = {} # TODO remove this sim_time_bool = Parameter('use_sim_time', Parameter.Type.BOOL, True) self.set_parameters([sim_time_bool]) self.task_states_cache = {}
def __init__(self): super().__init__('simple_trajectory') # Parameters self.length_m = self.declare_parameter("length_m", 20.0).value self.min_speed_mps = self.declare_parameter("min_speed_mps", 1.0).value self.max_speed_mps = self.declare_parameter("max_speed_mps", 8.0).value self.discretization_m = self.declare_parameter("discretization_m", 0.5).value self.speed_increments = self.declare_parameter("speed_increments", 0.2).value self.offset_distnace_m = self.declare_parameter("offset_distnace_m", 0.0).value self.one_time = self.declare_parameter("one_time", False).value self.timer_period_sec = self.declare_parameter("timer_period_sec", 0.0).value if self.max_speed_mps < self.min_speed_mps: self.max_speed_mps = self.min_speed_mps # Publisher / Subscriber qos = QoSProfile(depth=10) if (self.one_time): qos.durability = QoSDurabilityPolicy.RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL qos.depth = 1 self.publisher_trajectory_ = self.create_publisher( Trajectory, "trajectory", qos ) self.subscriber_vehicle_kinematic_state_ = self.create_subscription( VehicleKinematicState, "vehicle_kinematic_state", self.vehicle_kinematic_state_cb, 0 ) # Local variables self.trajectory_msg_ = None self.vehicle_kinematic_state_msg_ = None if (not self.one_time): self.timer = self.create_timer(self.timer_period_sec, self.publish_trajectory)
def test_invalid_qos(self): with self.assertRaises(InvalidQoSProfileException): # No history or depth settings provided QoSProfile() with self.assertRaises(InvalidQoSProfileException): # History is KEEP_LAST, but no depth is provided QoSProfile(history=QoSHistoryPolicy.KEEP_LAST)
def __init__(self): super().__init__('jarvis_camera') self.declare_parameters( namespace='', parameters=[ ('width', 640), ('height', 480), ('camera_topic', "camera_image"), ('time_period', 0.2), ] ) timer_period = self.get_parameter("time_period").value self.width = self.get_parameter("width").value self.height = self.get_parameter("height").value qos_profile = QoSProfile(depth=1) qos_profile.history = QoSHistoryPolicy.KEEP_LAST qos_profile.reliability = QoSReliabilityPolicy.BEST_EFFORT self.pub = self.create_publisher(CompressedImage, self.get_parameter("camera_topic").value, qos_profile) self.timer = self.create_timer(timer_period, self.timer_callback) self.frame = None self.has_frame = False # self.bridge = CvBridge() #cv2.VideoCapture( # "nvarguscamerasrc ! video/x-raw(memory:NVMM), width=(int)1280, height=(int)720,format=(string)NV12, framerate=(fraction)24/1 ! nvvidconv flip-method=2 ! video/x-raw, format=(string)BGRx ! videoconvert ! video/x-raw, format=(string)BGR ! appsink") self.get_logger().info("topic: %s, w: %d, h: %d, period: %.3f"%(self.get_parameter("camera_topic").value, self.width, self.height, timer_period))
def retrieve_urdf(self, timeout_sec: float = 5) -> None: """Retrieve the URDF file from the /robot_description topic. Will raise an EnvironmentError if the topic is unavailable. """ self.logger.info('Retrieving URDF from "/robot_description"...') qos_profile = QoSProfile(depth=1) qos_profile.durability = QoSDurabilityPolicy.TRANSIENT_LOCAL self.urdf = None def urdf_received(msg: String): self.urdf = msg.data self.create_subscription( msg_type=String, topic='/robot_description', qos_profile=qos_profile, callback=urdf_received, ) rclpy.spin_once(self, timeout_sec=timeout_sec) if self.urdf is None: self.logger.error('Could not retrieve the URDF!') raise EnvironmentError('Could not retrieve the URDF!')
def __init__(self, node): self.node_ = node self.topic_uvec_ = "/ifm3d/camera/unit_vectors" self.topic_extr_ = "/ifm3d/camera/extrinsics" self.topic_dist_ = "/ifm3d/camera/distance" self.bridge_ = CvBridge() self.lock_ = threading.Lock() self.uvec_ = None self.extr_ = None self.sub_uvec_ = self.node_.create_subscription( Image, self.topic_uvec_, self.uvec_cb, QoSProfile(depth=1, reliability=QoSReliabilityPolicy.RELIABLE, durability=QoSDurabilityPolicy.TRANSIENT_LOCAL)) qos = QoSProfile(depth=2, reliability=QoSReliabilityPolicy.BEST_EFFORT, durability=QoSDurabilityPolicy.VOLATILE) self.sub_extr_ = self.node_.create_subscription( Extrinsics, self.topic_extr_, self.extr_cb, qos) self.sub_dist_ = self.node_.create_subscription( Image, self.topic_dist_, self.dist_cb, qos)
def __init__(self): super().__init__('rqt_ros2_knowledge_graph') self.update_sub = self.create_subscription( GraphUpdate, '/graph_updates', self.graph_update_callback, qos_profile=QoSProfile( history=QoSHistoryPolicy.RMW_QOS_POLICY_HISTORY_KEEP_LAST, depth=100, reliability=QoSReliabilityPolicy. RMW_QOS_POLICY_RELIABILITY_RELIABLE)) self.graph_pub = self.create_publisher( GraphUpdate, '/graph_updates', qos_profile=QoSProfile( history=QoSHistoryPolicy.RMW_QOS_POLICY_HISTORY_KEEP_LAST, depth=100, reliability=QoSReliabilityPolicy. RMW_QOS_POLICY_RELIABILITY_RELIABLE)) msg = GraphUpdate() msg.operation_type = 3 # msg.REQSYNC msg.element_type = 2 # msg.GRAPH print(self.get_clock().now().seconds_nanoseconds()) msg.stamp.sec = self.get_clock().now().seconds_nanoseconds()[0] msg.stamp.nanosec = self.get_clock().now().seconds_nanoseconds()[1] msg.node_id = self.get_name() self.graph_pub.publish(msg) self.nodes = [] self.edges = []
def __init__(self): super().__init__("repeater_py") # You can play around with the QOS here, I could not find a significant improvement # reliability = ReliabilityPolicy(ReliabilityPolicy.RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT) # history = HistoryPolicy(HistoryPolicy.RMW_QOS_POLICY_HISTORY_KEEP_LAST) # liveliness = LivelinessPolicy(LivelinessPolicy.RMW_QOS_POLICY_LIVELINESS_SYSTEM_DEFAULT) # durability = DurabilityPolicy(DurabilityPolicy.RMW_QOS_POLICY_DURABILITY_VOLATILE) # lifespan = Duration(seconds=0.5) # deadline = Duration(seconds=0.5) # depth = 1 # pub_qos = QoSProfile( # reliability=reliability, # depth=depth, # history=history, # liveliness=liveliness, # durability=durability, # lifespan=lifespan, # deadline=deadline, # ) # As the QOS doesn't really make any difference, lets keep the default one pub_qos = QoSProfile(depth=10) sub_qos = QoSProfile(depth=10) self._pc_sub = self.create_subscription(PointCloud2, topic="pc_source", callback=self._handle_pc, qos_profile=sub_qos) self._pc_pub = self.create_publisher(PointCloud2, topic="repeated_pc_py", qos_profile=pub_qos)
def __init__(self): self.node = ros2.create_node("imu_sub") qos_profile = QoSProfile(depth=1) qos_profile.history = QoSHistoryPolicy.RMW_QOS_POLICY_HISTORY_KEEP_LAST qos_profile.reliability = ( QoSReliabilityPolicy.RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT ) qos_profile.durability = QoSDurabilityPolicy.RMW_QOS_POLICY_DURABILITY_VOLATILE # subscribe to the accel topic, which will call # __accel_callback every time the camera publishes data self.__accel_sub = self.node.create_subscription( Imu, self.__ACCEL_TOPIC, self.__accel_callback, qos_profile ) # subscribe to the gyro topic, which will call # __gyro_callback every time the camera publishes data self.__gyro_sub = self.node.create_subscription( Imu, self.__GYRO_TOPIC, self.__gyro_callback, qos_profile ) self.__acceleration = np.array([0, 0, 0]) self.__acceleration_buffer = deque() self.__angular_velocity = np.array([0, 0, 0]) self.__angular_velocity_buffer = deque()
def test_deprecation_warnings(self): with warnings.catch_warnings(record=True) as w: warnings.simplefilter('always') QoSProfile() assert len( w ) == 2 # must supply depth or history, _and_ KEEP_LAST needs depth assert issubclass(w[0].category, UserWarning) with warnings.catch_warnings(record=True) as w: warnings.simplefilter('always') # No deprecation if history is supplied QoSProfile( history=QoSHistoryPolicy.RMW_QOS_POLICY_HISTORY_KEEP_ALL) assert len(w) == 0, str(w[-1].message) with warnings.catch_warnings(record=True) as w: warnings.simplefilter('always') # Deprecation warning if KEEP_LAST, but no depth QoSProfile( history=QoSHistoryPolicy.RMW_QOS_POLICY_HISTORY_KEEP_LAST) assert len(w) == 1 assert issubclass(w[0].category, UserWarning) with warnings.catch_warnings(record=True) as w: warnings.simplefilter('always') # No deprecation if 'depth' is present QoSProfile( history=QoSHistoryPolicy.RMW_QOS_POLICY_HISTORY_KEEP_LAST, depth=1) assert len(w) == 0, str(w[-1].message) with warnings.catch_warnings(record=True) as w: warnings.simplefilter('always') # No deprecation if only depth QoSProfile(depth=1) assert len(w) == 0, str(w[-1].message)
def __init__(self): super().__init__("simple_publisher") logger = self.get_logger() self._num = 0 period = self.declare_parameter("period", 1.0) depth = self.declare_parameter("qos_depth", 5) logger.info("param period={}" .format(period.get_parameter_value().double_value)) logger.info("param qos_depth={}" .format(depth.get_parameter_value().integer_value)) profile = QoSProfile(depth=depth.get_parameter_value().integer_value) profile.reliability = QoSReliabilityPolicy.BEST_EFFORT profile.durability = QoSDurabilityPolicy.TRANSIENT_LOCAL profile.liveliness = QoSLivelinessPolicy.AUTOMATIC self._pub = self.create_publisher(SimpleMsg, "sample_topic", profile) self.create_timer( period.get_parameter_value().double_value, self.on_timer_callback) logger.info("publisher start!")
def __init__(self): self.__bridge = CvBridge() # ROS node self.node = ros2.create_node("image_sub") qos_profile = QoSProfile(depth=1) qos_profile.history = QoSHistoryPolicy.RMW_QOS_POLICY_HISTORY_KEEP_LAST qos_profile.reliability = ( QoSReliabilityPolicy.RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT ) qos_profile.durability = QoSDurabilityPolicy.RMW_QOS_POLICY_DURABILITY_VOLATILE # subscribe to the color image topic, which will call # __color_callback every time the camera publishes data self.__color_image_sub = self.node.create_subscription( Image, self.__COLOR_TOPIC, self.__color_callback, qos_profile ) self.__color_image = None self.__color_image_new = None # subscribe to the depth image topic, which will call # __depth_callback every time the camera publishes data self.__depth_image_sub = self.node.create_subscription( Image, self.__DEPTH_TOPIC, self.__depth_callback, qos_profile ) self.__depth_image = None self.__depth_image_new = None
def test_default_profile(self): with warnings.catch_warnings(record=True): warnings.simplefilter('always') profile = QoSProfile() assert all( profile.__getattribute__(k) == _qos_profile_default.__getattribute__(k) for k in profile.__slots__)
def __init__(self): super().__init__('base_info_handler') self._world_link = "/map" self._base_link = "/base_footprint" self._last_cmd = TRVCommand() try: self.declare_parameter(self.URDF_PARAM) except AttributeError: pass # ROS 2 prior to dashing does not have this method, so ignore AttributeErrors urdf_filename = self.get_parameter(self.URDF_PARAM).value get_logger(self.get_name()).info("Robot URDF: %s" % urdf_filename) self._odom = Odometry( header=Header(frame_id=self._world_link), child_frame_id=self._base_link, pose=PoseWithCovariance( pose=Pose(position=Point(x=0.0, y=0.0, z=0.0), orientation=Quaternion(x=0.0, y=0.0, z=0.0, w=1.0)))) self._transform = TransformStamped( header=Header(frame_id=self._world_link), child_frame_id=self._base_link) self.sub_robot_pose = self.create_subscription( BaseInfo, "/base_info", self.base_info_cb, qos_profile=qos_profile_sensor_data) # these bridge from standard geometry_msgs/Twist to drive_base_msgs/TRVCommand self.pub_cmd = self.create_publisher(TRVCommand, "/drive_cmd", qos_profile=QoSProfile(depth=1)) self.sub_joy = self.create_subscription( Joy, "/joy", self.joy_cb, qos_profile=QoSProfile(depth=1)) # standard odom topic. every message replaces the last, so we only need one self.pub_odom = self.create_publisher(Odometry, "/odom", qos_profile=QoSProfile(depth=1)) # tf topic. messages build a history, so give subscribers a chance to get them all # CHECKME: use transient local? self.pub_tf = self.create_publisher(TFMessage, "/tf", qos_profile=QoSProfile(depth=10)) # publish URDF "latched" -- doesn't seem to work, though if urdf_filename is not None: self.pub_urdf = self.create_publisher( String, "/robot_description", qos_profile=QoSProfile( depth=1, durability=QoSDurabilityPolicy. RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL)) self._urdf_msg = String(data=urdf_filename) self.pub_urdf.publish(self._urdf_msg) self.timer_desc = self.create_timer(1, self.description_timer_cb)
def test_default_incompatible_qos_callbacks(self): original_logger = rclpy.logging._root_logger pub_log_msg = None sub_log_msg = None log_msgs_future = Future() class MockLogger: def get_child(self, name): return self def warn(self, message, once=False): nonlocal pub_log_msg, sub_log_msg, log_msgs_future if message.startswith('New subscription discovered'): pub_log_msg = message elif message.startswith('New publisher discovered'): sub_log_msg = message if pub_log_msg is not None and sub_log_msg is not None: log_msgs_future.set_result(True) rclpy.logging._root_logger = MockLogger() qos_profile_publisher = QoSProfile( depth=10, durability=QoSDurabilityPolicy.RMW_QOS_POLICY_DURABILITY_VOLATILE) self.node.create_publisher(EmptyMsg, self.topic_name, qos_profile_publisher) message_callback = Mock() qos_profile_subscription = QoSProfile( depth=10, durability=QoSDurabilityPolicy. RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL) self.node.create_subscription(EmptyMsg, self.topic_name, message_callback, qos_profile_subscription) executor = rclpy.executors.SingleThreadedExecutor(context=self.context) rclpy.spin_until_future_complete(self.node, log_msgs_future, executor, 10.0) if not self.is_fastrtps: self.assertEqual( pub_log_msg, "New subscription discovered on topic '{}', requesting incompatible QoS. " 'No messages will be sent to it. ' 'Last incompatible policy: DURABILITY_QOS_POLICY'.format( self.topic_name)) self.assertEqual( sub_log_msg, "New publisher discovered on topic '{}', offering incompatible QoS. " 'No messages will be received from it. ' 'Last incompatible policy: DURABILITY_QOS_POLICY'.format( self.topic_name)) rclpy.logging._root_logger = original_logger
def main(args=None): rclpy.init(args=args) node = rclpy.create_node('qos_subscription') counter = Counter() sub_default = node.create_subscription( String, 'topic', lambda msg: counter.onMsg('default', msg), QoSProfile(depth=10)) sub_transient = node.create_subscription( String, 'topic', lambda msg: counter.onMsg('transient', msg), QoSProfile( depth=10, durability=QoSDurabilityPolicy.RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL)) sub_volatile = node.create_subscription( String, 'topic', lambda msg: counter.onMsg('volatile', msg), QoSProfile( depth=10, durability=QoSDurabilityPolicy.RMW_QOS_POLICY_DURABILITY_VOLATILE)) sub_reliable = node.create_subscription( String, 'topic', lambda msg: counter.onMsg('reliable', msg), QoSProfile( depth=10, reliability=QoSReliabilityPolicy.RMW_QOS_POLICY_RELIABILITY_RELIABLE)) sub_best_effort = node.create_subscription( String, 'topic', lambda msg: counter.onMsg('best_effort', msg), QoSProfile( depth=10, reliability=QoSReliabilityPolicy.RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT)) # Asserts to suppress unused variable warnings. assert sub_default assert sub_transient assert sub_volatile assert sub_reliable assert sub_best_effort timer = node.create_timer(0.5, lambda: print(counter.count)) rclpy.spin(node) node.destroy_timer(timer) node.destroy_node() rclpy.shutdown()
def __init__(self): qos = QoSProfile(depth=10) qos.durability = DurabilityPolicy.RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL super().__init__('minimal_subscriber') self.subscription = self.create_subscription(String, 'topic', self.listener_callback, qos)
def __init__(self, robot_description): super().__init__('robot_desc_publisher') qos_profile = QoSProfile(depth=1) qos_profile.durability = QoSDurabilityPolicy.RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL self.publisher_ = self.create_publisher(String, 'robot_description', qos_profile) self.robot_description = robot_description self.timer_callback()
def __init__(self, test_type: TestType, initial_pose: Pose, goal_pose: Pose, namespace: str = ''): super().__init__(node_name='nav2_tester', namespace=namespace) self.initial_pose_pub = self.create_publisher( PoseWithCovarianceStamped, 'initialpose', 10) self.goal_pub = self.create_publisher(PoseStamped, 'goal_pose', 10) transient_local_qos = QoSProfile( durability=QoSDurabilityPolicy. RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL, reliability=QoSReliabilityPolicy. RMW_QOS_POLICY_RELIABILITY_RELIABLE, history=QoSHistoryPolicy.RMW_QOS_POLICY_HISTORY_KEEP_LAST, depth=1) volatile_qos = QoSProfile( durability=QoSDurabilityPolicy.RMW_QOS_POLICY_DURABILITY_VOLATILE, reliability=QoSReliabilityPolicy. RMW_QOS_POLICY_RELIABILITY_RELIABLE, history=QoSHistoryPolicy.RMW_QOS_POLICY_HISTORY_KEEP_LAST, depth=1) self.model_pose_sub = self.create_subscription( PoseWithCovarianceStamped, 'amcl_pose', self.poseCallback, transient_local_qos) self.test_type = test_type self.filter_test_result = True if self.test_type == TestType.KEEPOUT: self.plan_sub = self.create_subscription(Path, 'plan', self.planCallback, volatile_qos) elif self.test_type == TestType.SPEED: self.speed_it = 0 # Expected chain of speed limits self.limits = [50.0, 0.0] # Permissive array: all received speed limits must match to "limits" from above self.limit_passed = [False, False] self.plan_sub = self.create_subscription(SpeedLimit, 'speed_limit', self.speedLimitCallback, volatile_qos) self.mask_received = False self.mask_sub = self.create_subscription(OccupancyGrid, 'filter_mask', self.maskCallback, transient_local_qos) self.initial_pose_received = False self.initial_pose = initial_pose self.goal_pose = goal_pose self.action_client = ActionClient(self, NavigateToPose, 'navigate_to_pose')
def __init__(self, buffer, node, *, spin_thread=False, qos=None, static_qos=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, device_key, wb_device, params=None): super().__init__(node, device_key, wb_device, params) self._camera_info_publisher = None self._image_publisher = None # Create topics if not self._disable: self._image_publisher = self._node.create_publisher( Image, self._topic_name + '/image_raw', QoSProfile( depth=1, reliability=QoSReliabilityPolicy.RELIABLE, durability=DurabilityPolicy.TRANSIENT_LOCAL, history=HistoryPolicy.KEEP_LAST, )) self._camera_info_publisher = self._node.create_publisher( CameraInfo, self._topic_name + '/camera_info', QoSProfile( depth=1, reliability=QoSReliabilityPolicy.RELIABLE, durability=DurabilityPolicy.TRANSIENT_LOCAL, history=HistoryPolicy.KEEP_LAST, )) # CameraInfo data self.__message_info = CameraInfo() self.__message_info.header.stamp = Time( seconds=self._node.robot.getTime()).to_msg() self.__message_info.height = self._wb_device.getHeight() self.__message_info.width = self._wb_device.getWidth() self.__message_info.distortion_model = 'plumb_bob' focal_length = self._wb_device.getFocalLength() if focal_length == 0: focal_length = 570.34 # Identical to Orbbec Astra self.__message_info.d = [0.0, 0.0, 0.0, 0.0, 0.0] self.__message_info.r = [ 1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0 ] self.__message_info.k = [ focal_length, 0.0, self._wb_device.getWidth() / 2, 0.0, focal_length, self._wb_device.getHeight() / 2, 0.0, 0.0, 1.0 ] self.__message_info.p = [ focal_length, 0.0, self._wb_device.getWidth() / 2, 0.0, 0.0, focal_length, self._wb_device.getHeight() / 2, 0.0, 0.0, 0.0, 1.0, 0.0 ] self._camera_info_publisher.publish(self.__message_info) # Load parameters camera_period_param = node.declare_parameter( wb_device.getName() + '_period', self._timestep) self._camera_period = camera_period_param.value
def __init__(self, node_name, queue_size=10, latch=False, rospy_init=True, **kwargs): param = Parameter('use_sim_time', Parameter.Type.BOOL, True) super(CompatibleNode, self).__init__(node_name, allow_undeclared_parameters=True, automatically_declare_parameters_from_overrides=True, parameter_overrides=[param], **kwargs) if latch: self.qos_profile = QoSProfile( depth=queue_size, durability=QoSDurabilityPolicy.RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL) else: self.qos_profile = QoSProfile(depth=queue_size) self.callback_group = None
def test_eq_operator(self): profile_1 = QoSProfile() profile_same = QoSProfile() profile_different_number = QoSProfile(depth=1) profile_different_duration = QoSProfile(deadline=Duration(seconds=2)) profile_equal_duration = QoSProfile(deadline=Duration(seconds=2)) self.assertEqual(profile_1, profile_same) self.assertNotEqual(profile_1, profile_different_number) self.assertNotEqual(profile_1, profile_different_duration) self.assertEqual(profile_different_duration, profile_equal_duration)
def __init__(self): super().__init__('nanosaur') # Initialize eyes controller self.eyes = eyes(self) # Get rate joint_states self.declare_parameter("rate", 5) self.timer_period = 1. / float(self.get_parameter("rate").value) # Get RPM motors self.declare_parameter("rpm", 150) self.rpm = int(self.get_parameter("rpm").value) # Get parameter left wheel name # https://index.ros.org/doc/ros2/Tutorials/Using-Parameters-In-A-Class-Python/ self.declare_parameter("motor.left.channel", 1) left_id = int(self.get_parameter("motor.left.channel").value) self.declare_parameter("motor.left.wheel", "sprocket_left_joint") self.left_wheel_name = self.get_parameter("motor.left.wheel").value # Get parameter right wheel name self.declare_parameter("motor.right.channel", 4) right_id = int(self.get_parameter("motor.right.channel").value) self.declare_parameter("motor.right.wheel", "sprocket_right_joint") self.right_wheel_name = self.get_parameter("motor.right.wheel").value # Motor info message self.get_logger().info( f"RPM motors {self.rpm} - timer {self.timer_period}") self.get_logger().info( f"Motor left: Channel {left_id} - Wheel {self.left_wheel_name}") self.get_logger().info( f"Motor Right: Channel {right_id} - Wheel {self.right_wheel_name}") # Load motors self.mright = Motor(right_id, self.rpm) self.mleft = Motor(left_id, self.rpm) # Load subscriber robot_description self.create_subscription( String, 'robot_description', lambda msg: self.configure_robot(msg.data), QoSProfile(depth=1, durability=rclpy.qos.QoSDurabilityPolicy. RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL)) # joint state controller # https://index.ros.org/doc/ros2/Tutorials/URDF/Using-URDF-with-Robot-State-Publisher/ self.joint_state = JointState() qos_profile = QoSProfile(depth=10) self.joint_pub = self.create_publisher(JointState, 'joint_states', qos_profile) self.timer = self.create_timer(self.timer_period, self.transform_callback) # Drive control self.p = [0.0, 0.0] # [right, left] self.r = [0.0, 0.0] # [right, left] self.subscription = self.create_subscription(Twist, 'cmd_vel', self.drive_callback, 10) self.subscription # prevent unused variable warning # Node started self.get_logger().info("Hello NanoSaur!")
def main(): rclpy.init() node = rclpy.create_node('HDMAP_Server') qos_profile = QoSProfile(depth=1) qos_profile.history = QoSHistoryPolicy.RMW_QOS_POLICY_HISTORY_KEEP_LAST #hdmap_pub = node.create_publisher(HdmapMarker, 'hdmap_marker') hdmapA_pub = node.create_publisher(HdmapArray, 'hdmap_array') dgistMap = loadMap("dgist_a1.geojson").getItem() dgistMap = dgistMap.fillna('nodata') curb = dgistMap.loc[dgistMap['type'] == HdmapMarker.CURB] whiteline = dgistMap.loc[dgistMap['type'] == HdmapMarker.WHITELINE] dgistMap.loc[dgistMap['type'] == HdmapMarker.TRAFFICSIGN] hdmap_array = HdmapArray() hdmap_array.array = [] ros_time = node.get_clock().now().to_msg() featuredMap = dgistMap.loc[dgistMap['type'] != 'Feature'] dataLen = len(featuredMap) for idx in range(0, dataLen): data = featuredMap.iloc[idx] marker = HdmapMarker() marker.id = idx marker.scale = Vector3(x=0.2, y=0.2, z=0.2) print(data) marker.type = int(data['type']) marker.header.stamp = ros_time # TODO: support another specific type try: if marker.type == marker.TRAFFICSIGN: coord_x, coord_y = data['geometry'].xy marker.pose = Point(x=float(coord_x), y=float(coord_y)) else: marker.points = [] coord_x, coord_y = data['geometry'].xy for lat, lng in zip(coord_x, coord_x): marker.points.append(Point(x=float(lat), y=float(lng))) except Exception: pass hdmap_array.array.append(marker) hdmap_array.header.stamp = ros_time import time while(1): hdmapA_pub.publish(hdmap_array) time.sleep(0.5)
def test_big_nanoseconds(self): # Under 31 bits no_problem = QoSProfile(lifespan=Duration(seconds=2)) self.convert_and_assert_equality(no_problem) # Total nanoseconds in duration is too large to store in 32 bit signed int int32_problem = QoSProfile(lifespan=Duration(seconds=4)) self.convert_and_assert_equality(int32_problem) # Total nanoseconds in duration is too large to store in 32 bit unsigned int uint32_problem = QoSProfile(lifespan=Duration(seconds=5)) self.convert_and_assert_equality(uint32_problem)
def _qos_profile(history, depth, reliability, durability): # depth profile = QoSProfile(depth=depth) # history if history.lower() == "keep_all": profile.history = QoSHistoryPolicy.RMW_QOS_POLICY_HISTORY_KEEP_ALL elif history.lower() == "keep_last": profile.history = QoSHistoryPolicy.RMW_QOS_POLICY_HISTORY_KEEP_LAST else: raise RuntimeError("Invalid history policy: %s" % history) # reliability if reliability.lower() == "reliable": profile.reliability = \ QoSReliabilityPolicy.RMW_QOS_POLICY_RELIABILITY_RELIABLE elif reliability.lower() == "best_effort": profile.reliability = \ QoSReliabilityPolicy.RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT else: raise RuntimeError("Invalid reliability policy: %s" % reliability) # durability if durability.lower() == "transient_local": profile.durability = \ QoSDurabilityPolicy.RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL elif durability.lower() == "volatile": profile.durability = \ QoSDurabilityPolicy.RMW_QOS_POLICY_DURABILITY_VOLATILE else: raise RuntimeError("Invalid durability policy: %s" % durability) return profile
def run_topic_listening(node, topic_monitor, options): """Subscribe to relevant topics and manage the data received from susbcriptions.""" already_ignored_topics = set() while rclpy.ok(): # Check if there is a new topic online # TODO(dhood): use graph events rather than polling topic_names_and_types = node.get_topic_names_and_types() for topic_name, type_names in topic_names_and_types: # Infer the appropriate QoS profile from the topic name topic_info = topic_monitor.get_topic_info(topic_name) if topic_info is None: # The topic is not for being monitored continue if len(type_names) != 1: if topic_name not in already_ignored_topics: node.get_logger().info( "Warning: ignoring topic '%s', which has more than one type: [%s]" % (topic_name, ', '.join(type_names))) already_ignored_topics.add(topic_name) continue type_name = type_names[0] if not topic_monitor.is_supported_type(type_name): if topic_name not in already_ignored_topics: node.get_logger().info( "Warning: ignoring topic '%s' because its message type (%s)" 'is not suported.' % (topic_name, type_name)) already_ignored_topics.add(topic_name) continue is_new_topic = topic_name and topic_name not in topic_monitor.monitored_topics if is_new_topic: # Register new topic with the monitor qos_profile = QoSProfile(depth=10) qos_profile.depth = QOS_DEPTH if topic_info['reliability'] == 'best_effort': qos_profile.reliability = \ QoSReliabilityPolicy.BEST_EFFORT topic_monitor.add_monitored_topic(Header, topic_name, node, qos_profile, options.expected_period, options.allowed_latency, options.stale_time) # Wait for messages with a timeout, otherwise this thread will block any other threads # until a message is received rclpy.spin_once(node, timeout_sec=0.05)
def __init__(self): super().__init__('node3', namespace='name/space3') # Params #____________________________________________ # Publishers #____________________________________________ # Subscribers #____________________________________________ # sub1 # Qos profile qos_profile_sub1 = QoSPresetProfiles.SYSTEM_DEFAULT.value self.subscriber_sub1 = self.create_subscription( M1, 'topic/path1', self.subscriber_call_sub1, qos_profile=qos_profile_sub1) self.subscriber_sub1 #_____ # Servers #____________________________________________ # Clients #____________________________________________ # c2 # Qos profile qos_profile_c2 = QoSProfile( history=QoSHistoryPolicy.KEEP_ALL, durability=QoSDurabilityPolicy.SYSTEM_DEFAULT, reliability=QoSReliabilityPolicy.RELIABLE, depth=0) # Additional qos settings qos_profile_c2.liveliness = QoSLivelinessPolicy.SYSTEM_DEFAULT qos_profile_c2.deadline.sec = 0 qos_profile_c2.deadline.nsec = 0 qos_profile_c2.lifespan.sec = 0 qos_profile_c2.lifespan.nsec = 0 qos_profile_c2.liveliness_lease_duration.sec = 0 qos_profile_c2.liveliness_lease_duration.nsec = 0 qos_profile_c2.avoid_ros_namespace_conventions = False self.client_c2 = self.create_client(Addtwo, 'service2', qos_profile=qos_profile_c2)