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 __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 __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 __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 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 qos_profile(d): history = d["history"] # keep_last|keep_all depth = d["depth"] # used if using keep_last reliability = d["reliability"] # reliable|best_effort durability = d["durability"] # transient_local|volatile # depth profile = QoSProfile(depth = depth) # history if history == "keep_all": profile.history = QoSHistoryPolicy.RMW_QOS_POLICY_HISTORY_KEEP_ALL elif history == "keep_last": profile.history = QoSHistoryPolicy.RMW_QOS_POLICY_HISTORY_KEEP_LAST else: raise RuntimeError("Invalid history policy: %s"%history) # reliability if reliability == "reliable": profile.reliability = \ QoSReliabilityPolicy.RMW_QOS_POLICY_RELIABILITY_RELIABLE elif reliability == "best_effort": profile.reliability = \ QoSReliabilityPolicy.RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT else: raise RuntimeError("Invalid reliability policy: %s"%reliability) # durability if durability == "transient_local": profile.durability = \ QoSDurabilityPolicy.RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL elif durability == "volatile": profile.durability = \ QoSDurabilityPolicy.RMW_QOS_POLICY_DURABILITY_VOLATILE else: raise RuntimeError("Invalid durability policy: %s"%durability) return profile
def __init__(self): super().__init__('minimal_publisher') qos_at_start = QoSProfile() print("Configuring", end=',') qos_at_start.reliability = QoSReliabilityPolicy.RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT qos_at_start.depth = 10 qos_at_start.history = QoSHistoryPolicy.RMW_QOS_POLICY_HISTORY_KEEP_ALL qos_at_start.durability = QoSDurabilityPolicy.RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL print("Configured") #self.get_logger().info('Publishing: "%s"' % qos_profile_at_start) #print(" ", qos_at_start) self.publisher_ = self.create_publisher(String, 'topic', qos_profile=qos_at_start) print(self.publisher_) timer_period = 0.5 # seconds self.timer = self.create_timer(timer_period, self.timer_callback) self.i = 0
def __init__(self): super().__init__('minimal_subscriber') qos_at_start = QoSProfile() qos_at_start.reliability = QoSReliabilityPolicy.RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT #qos_at_start.reliability = QoSReliabilityPolicy.RMW_QOS_POLICY_RELIABILITY_RELIABLE qos_at_start.depth = 10 qos_at_start.history = QoSHistoryPolicy.RMW_QOS_POLICY_HISTORY_KEEP_ALL qos_at_start.durability = QoSDurabilityPolicy.RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL self.group = ThrottledCallbackGroup(self) self.subscription = self.create_subscription(String, 'topic', self.listener_callback, qos_profile=qos_at_start, callback_group=self.group) print(self.subscription) self.subscription # prevent unused variable warning
def __init__(self): super().__init__('camera_viewer') self.declare_parameters(namespace='', parameters=[('time_period', 0.033), ('camera_topic', '/jarvis/camera') ]) self.timer = self.create_timer( self.get_parameter("time_period").value, self.timer_callback) qos_profile = QoSProfile(depth=1) qos_profile.history = QoSHistoryPolicy.KEEP_LAST qos_profile.reliability = QoSReliabilityPolicy.BEST_EFFORT self.sub = self.create_subscription( CompressedImage, self.get_parameter("camera_topic").value, self.callback, qos_profile) self.frame = None self.has_frame = False
def __init__(self): print("Env initialized") #define spaces self.max_v = 9.5 self.min_v = -9.5 self.max_obs = 2.0 self.min_obs = -2.0 self.iter = 0.0 self.time = 0 self.time_0 = 0 #Parameters Reward function self.a = 1000 self.b = 35 self.action_space = spaces.Box(self.min_v, self.max_v, shape=(4, ), dtype=np.float32) self.observation_space = spaces.Box(self.min_obs, self.max_obs, shape=(12, ), dtype=np.float32) self.reward_range = (-np.inf, 0) #create ROS2 node rclpy.init(args=None) self.node = rclpy.create_node("pr_env") #initialize variables self.target_position = np.array( [0.830578, 0.847096, 0.831132, 0.792133]) first_position = np.array([0.679005, 0.708169, 0.684298, 0.637145]) first_velocity = np.array([0.0, 0.0, 0.0, 0.0]) self.first_obs = np.concatenate( (first_position, first_velocity, (self.target_position - first_position)), axis=0) self.obs = self.first_obs self.status = "starting" self.time_obs_ns = 0 self.time_obs_ns_ = self.time_obs_ns #create ROS2 communication self.publisher_ = self.node.create_publisher(PRArrayH, 'control_action', 1) self.subscription_ = self.node.create_subscription( PRState, 'pr_state', self._state_callback, 1) self.publisher_reset_vel = self.node.create_publisher( Bool, 'der_reset', 1) sim_qos = QoSProfile(depth=1) sim_qos.reliability = QoSReliabilityPolicy.BEST_EFFORT sim_qos.history = QoSHistoryPolicy.KEEP_LAST sim_qos.durability = QoSDurabilityPolicy.VOLATILE self.subscription_sim_ = self.node.create_subscription( Quaternion, "posicion_sim", self._sim_callback, sim_qos) #create pub and sub for matlab macro self.publisher_start_ = self.node.create_publisher( Bool, "start_flag", 1) self.subscription_end_ = self.node.create_subscription( String, "status_sim", self._end_callback, 1) self.seed() start = input('Press any key to start')
def main(): parser = argparse.ArgumentParser( formatter_class=argparse.ArgumentDefaultsHelpFormatter) parser.add_argument( 'data_name', nargs='?', default='topic1', help='Name of the data (must comply with ROS topic rules)') parser.add_argument('--best-effort', action='store_true', default=False, help="Set QoS reliability option to 'best effort'") parser.add_argument('--transient-local', action='store_true', default=False, help="Set QoS durability option to 'transient local'") parser.add_argument('--depth', type=int, default=default_depth, help='Size of the QoS history depth') parser.add_argument( '--keep-all', action='store_true', default=False, help= "Set QoS history option to 'keep all' (unlimited depth, subject to resource limits)" ) parser.add_argument('--payload-size', type=int, default=0, help='Size of data payload to send') parser.add_argument('--period', type=float, default=0.5, help='Time in seconds between messages') parser.add_argument( '--end-after', type=int, help='Script will exit after publishing this many messages') args = parser.parse_args() reliability_suffix = '_best_effort' if args.best_effort else '' topic_name = '{0}_data{1}'.format(args.data_name, reliability_suffix) rclpy.init() node = rclpy.create_node('%s_pub' % topic_name) node_logger = node.get_logger() qos_profile = QoSProfile(depth=args.depth) if args.best_effort: node_logger.info('Reliability: best effort') qos_profile.reliability = QoSReliabilityPolicy.BEST_EFFORT else: node_logger.info('Reliability: reliable') qos_profile.reliability = QoSReliabilityPolicy.RELIABLE if args.keep_all: node_logger.info('History: keep all') qos_profile.history = QoSHistoryPolicy.KEEP_ALL else: node_logger.info('History: keep last') qos_profile.history = QoSHistoryPolicy.KEEP_LAST node_logger.info('Depth: {0}'.format(args.depth)) if args.transient_local: node_logger.info('Durability: transient local') qos_profile.durability = QoSDurabilityPolicy.TRANSIENT_LOCAL else: node_logger.info('Durability: volatile') qos_profile.durability = QoSDurabilityPolicy.VOLATILE data_pub = node.create_publisher(Header, topic_name, qos_profile) node_logger.info('Publishing on topic: {0}'.format(topic_name)) node_logger.info('Payload size: {0}'.format(args.payload_size)) data = 'a' * args.payload_size msg = Header() cycle_count = 0 def publish_msg(val): msg.frame_id = '{0}_{1}'.format(val, data) data_pub.publish(msg) node_logger.info('Publishing: "{0}"'.format(val)) while rclpy.ok(): if args.end_after is not None and cycle_count >= args.end_after: publish_msg(-1) sleep(0.1) # allow reliable messages to get re-sent if needed return publish_msg(cycle_count) cycle_count += 1 try: sleep(args.period) except KeyboardInterrupt: publish_msg(-1) sleep(0.1) raise
def __init__(self): """Create a SoftwareUpdateNode. """ super().__init__("software_update_process") self.get_logger().info("software_update_process started") # Scheduler to queue the function calls and run them in a separate thread. self.scheduler = scheduler.Scheduler(self.get_logger()) # Threading lock object to safely update the update_state variable. self.state_guard = threading.Lock() self.update_state = software_update_config.SoftwareUpdateState.UPDATE_UNKNOWN # Threading Event object to indicate if the software update check is completed. self.check_complete = threading.Event() # Flag to indicate software update check in progress. self.check_in_progress = False # List of packages that have updates available. self.update_list = list() # Flag to identify if the network is connected. self.is_network_connected = False # Flag to identify if software update check has to performed # again when network is connected. self.reschedule_software_update_check = False # The apt cache object. self.cache = apt.Cache() # Double buffer object containing the current update percentage and progress state. self.pct_dict_db = utility.DoubleBuffer(clear_data_on_get=False) self.pct_dict_db.put({ software_update_config.PROG_STATE_KEY: software_update_config.PROGRESS_STATES[0], software_update_config.PROG_PCT_KEY: 0.0 }) # Timer to periodically check for software update. if software_update_config.ENABLE_PERIODIC_SOFTWARE_UPDATE: self.get_logger().info( "Schedule the software update check every " f"{software_update_config.SOFTWARE_UPDATE_PERIOD_IN_SECONDS} " "seconds.") self.schedule_update_check_cb = ReentrantCallbackGroup() self.update_check_timer = \ self.create_timer(software_update_config.SOFTWARE_UPDATE_PERIOD_IN_SECONDS, self.schedule_update_check, callback_group=self.schedule_update_check_cb) # Publisher that sends software update pct and status. # Guaranteed delivery is needed to send messages to late-joining subscription. qos_profile = QoSProfile(depth=1) qos_profile.history = QoSHistoryPolicy.KEEP_LAST qos_profile.reliability = QoSReliabilityPolicy.RELIABLE self.pct_obj = SoftwareUpdatePctMsg() self.software_update_pct_pub_cb = ReentrantCallbackGroup() self.software_update_pct_publisher = \ self.create_publisher(SoftwareUpdatePctMsg, software_update_config.SOFTWARE_UPDATE_PCT_TOPIC_NAME, qos_profile=qos_profile, callback_group=self.software_update_pct_pub_cb) # Service to check if there is a software update available. self.software_update_check_cb_group = ReentrantCallbackGroup() self.software_update_check_service = \ self.create_service(SoftwareUpdateCheckSrv, software_update_config.SOFTWARE_UPDATE_CHECK_SERVICE_NAME, self.software_update_check, callback_group=self.software_update_check_cb_group) # Service to execute the software update and install latest packages. self.begin_update_service_cb_group = ReentrantCallbackGroup() self.begin_update_service = \ self.create_service(BeginSoftwareUpdateSrv, software_update_config.BEGIN_SOFTWARE_UPDATE_SERVICE_NAME, self.begin_update, callback_group=self.begin_update_service_cb_group) # Service to get the current software update state. self.software_update_state_service_cb_group = ReentrantCallbackGroup() self.software_update_state_service = \ self.create_service(SoftwareUpdateStateSrv, software_update_config.SOFTWARE_UPDATE_STATE_SERVICE_NAME, self.software_update_state, callback_group=self.software_update_state_service_cb_group) self.nw_con_status_cb_group = ReentrantCallbackGroup() self.network_connection_status_sub = \ self.create_subscription(NetworkConnectionStatus, software_update_config.NETWORK_CONNECTION_STATUS_TOPIC_NAME, self.network_connection_status_cb, 10, callback_group=self.nw_con_status_cb_group) # Clients to Status LED services that are called to indicate progress/success/failure # status while loading model. self.led_cb_group = MutuallyExclusiveCallbackGroup() self.led_blink_service = self.create_client( SetStatusLedBlinkSrv, constants.LED_BLINK_SERVICE_NAME, callback_group=self.led_cb_group) self.led_solid_service = self.create_client( SetStatusLedSolidSrv, constants.LED_SOLID_SERVICE_NAME, callback_group=self.led_cb_group) while not self.led_blink_service.wait_for_service(timeout_sec=1.0): self.get_logger().info( "Led blink service not available, waiting again...") while not self.led_solid_service.wait_for_service(timeout_sec=1.0): self.get_logger().info( "Led solid service not available, waiting again...") self.led_blink_request = SetStatusLedBlinkSrv.Request() self.led_solid_request = SetStatusLedSolidSrv.Request() # Client to USB File system subscription service that allows the node to install # signed software packages(*.deb.gpg) present in the USB. The usb_monitor_node # will trigger notification if it finds the "update" folder from the watchlist # in the USB drive. self.usb_sub_cb_group = ReentrantCallbackGroup() self.usb_file_system_subscribe_client = self.create_client( USBFileSystemSubscribeSrv, constants.USB_FILE_SYSTEM_SUBSCRIBE_SERVICE_NAME, callback_group=self.usb_sub_cb_group) while not self.usb_file_system_subscribe_client.wait_for_service( timeout_sec=1.0): self.get_logger().info( "File System Subscribe not available, waiting again...") # Client to USB Mount point manager service to indicate that the usb_monitor_node can safely # decrement the counter for the mount point once the action function for the "update" folder # file being watched by software_update_node is succesfully executed. self.usb_mpm_cb_group = ReentrantCallbackGroup() self.usb_mount_point_manager_client = self.create_client( USBMountPointManagerSrv, constants.USB_MOUNT_POINT_MANAGER_SERVICE_NAME, callback_group=self.usb_mpm_cb_group) while not self.usb_mount_point_manager_client.wait_for_service( timeout_sec=1.0): self.get_logger().info( "USB mount point manager service not available, waiting again..." ) # Subscriber to USB File system notification publisher to recieve the broadcasted messages # with file/folder details, whenever a watched file is identified in the USB connected. self.usb_notif_cb_group = ReentrantCallbackGroup() self.usb_file_system_notification_sub = self.create_subscription( USBFileSystemNotificationMsg, constants.USB_FILE_SYSTEM_NOTIFICATION_TOPIC, self.usb_file_system_notification_cb, 10, callback_group=self.usb_notif_cb_group) # Add the "update" folder to the watchlist. usb_file_system_subscribe_request = USBFileSystemSubscribeSrv.Request() usb_file_system_subscribe_request.file_name = software_update_config.UPDATE_SOURCE_DIRECTORY usb_file_system_subscribe_request.callback_name = software_update_config.SCHEDULE_USB_UPDATE_SCAN_CB usb_file_system_subscribe_request.verify_name_exists = True self.usb_file_system_subscribe_client.call_async( usb_file_system_subscribe_request) # Set the power led to blue. self.led_solid_request.led_index = constants.LEDIndex.POWER_LED self.led_solid_request.color = constants.LEDColor.BLUE self.led_solid_request.hold = 0.0 self.led_solid_service.call_async(self.led_solid_request) # Heartbeat timer. self.timer_count = 0 self.timer = self.create_timer(5.0, self.timer_callback) # Schedule update check. if software_update_config.ENABLE_PERIODIC_SOFTWARE_UPDATE: self.schedule_update_check() self.get_logger().info("Software Update node successfully created")
def main(): rclpy.init() node = rclpy.create_node('OSMDrawer') qos_profile = QoSProfile(depth=1) qos_profile.history = QoSHistoryPolicy.RMW_QOS_POLICY_HISTORY_KEEP_LAST tfPub = node.create_publisher(TFMessage, '/tf', qos_profile=qos_profile) mapPub = node.create_publisher(Marker, 'mapPub', qos_profile=qos_profile) mapPub2 = node.create_publisher(Marker, 'mapPub2', qos_profile=qos_profile) autoStuff = loadMap("AutonomouStuff_20191119_134123.geojson") mapfile = autoStuff.getItem() parkdf = mapfile.loc[mapfile['subtype'] == ('parking')] noparkdf = mapfile.loc[mapfile['subtype'] != ('parking')] fig = plt.figure() ax = fig.add_subplot(1, 1, 1) #Marker Array marker_park = makerarray_frame(node) for part in parkdf.geometry: if type(part) == shapely.geometry.multilinestring.MultiLineString: for line in part: lat, lng = line.xy ax.plot(lat, lng, color='blue') for lat, lng in zip(lat, lng): marker_park.drawPoint(Point(x=lat, y=lng, z=0.), ColorRGBA(r=0.5, g=0.5, b=0., a=1.0)) continue # https://gis.stackexchange.com/questions/104312/multilinestring-to-separate-individual-lines-using-python-with-gdal-ogr-fiona lat, lng = part.xy ax.plot(lat, lng, color='blue') for lat, lng in zip(lat, lng): marker_park.drawPoint(Point(x=lat, y=lng, z=0.), ColorRGBA(r=0.5, g=0.5, b=0., a=1.0)) marker_nopark = makerarray_frame(node) for part in noparkdf.geometry: if type(part) == shapely.geometry.multilinestring.MultiLineString: for line in part: lat, lng = line.xy ax.plot(lat, lng, color='green') for lat, lng in zip(lat, lng): marker_nopark.drawPoint(Point(x=lat, y=lng, z=0.)) continue # https://gis.stackexchange.com/questions/104312/multilinestring-to-separate-individual-lines-using-python-with-gdal-ogr-fiona lat, lng = part.xy ax.plot(lat, lng, color='green') for lat, lng in zip(lat, lng): marker_nopark.drawPoint(Point(x=lat, y=lng, z=0.)) plt.show() while (rclpy.ok()): marker_nopark.publish(mapPub2, tfPub) marker_park.publish(mapPub, tfPub)
def __init__(self): """Create a WebServerNode and launch a flask server with default host and port details. """ super().__init__("webserver_publisher_node") self.get_logger().info("webserver_publisher_node started") HOST_DEFAULT = "0.0.0.0" PORT_DEFAULT = "5001" self.get_logger().info(f"Running the flask server on {HOST_DEFAULT}:{PORT_DEFAULT}") # Run the Flask webserver as a background thread. self.get_logger().info("Running webserver") self.server_thread = threading.Thread(target=app.run, daemon=True, kwargs={ "host": HOST_DEFAULT, "port": PORT_DEFAULT, "use_reloader": False, "threaded": True} ) self.server_thread.start() # Create service clients. # Create a reentrant callback group to set the vehicle mode. vehicle_mode_cb_group = ReentrantCallbackGroup() self.get_logger().info(f"Create vehicle state service client: {VEHICLE_STATE_SERVICE}") self.vehicle_state_cli = self.create_client(ActiveStateSrv, VEHICLE_STATE_SERVICE, callback_group=vehicle_mode_cb_group) self.wait_for_service_availability(self.vehicle_state_cli) # Create a reentrant callback group to activate the state. enable_state_cb_group = ReentrantCallbackGroup() self.get_logger().info(f"Create enable state service client: {ENABLE_STATE_SERVICE}") self.enable_state_cli = self.create_client(EnableStateSrv, ENABLE_STATE_SERVICE, callback_group=enable_state_cb_group) self.wait_for_service_availability(self.enable_state_cli) # Create a reentrant callback group to get the calibration value. get_calibration_cb_group = ReentrantCallbackGroup() self.get_logger().info(f"Create get_cal service client: {GET_CAR_CAL_SERVICE}") self.get_cal_cli = self.create_client(GetCalibrationSrv, GET_CAR_CAL_SERVICE, callback_group=get_calibration_cb_group) self.wait_for_service_availability(self.get_cal_cli) # Create a mutually exclusive callback group to set the calibration value. set_calibration_cb_group = MutuallyExclusiveCallbackGroup() self.get_logger().info(f"Create set_cal service client: {SET_CAR_CAL_SERVICE}") self.set_cal_cli = self.create_client(SetCalibrationSrv, SET_CAR_CAL_SERVICE, callback_group=set_calibration_cb_group) self.wait_for_service_availability(self.set_cal_cli) # Create a reentrant callback group to get the device info values. get_device_info_cb_group = ReentrantCallbackGroup() self.get_logger().info(f"Create device info service client: {GET_DEVICE_INFO_SERVICE}") self.get_revision_info_cli = self.create_client(GetDeviceInfoSrv, GET_DEVICE_INFO_SERVICE, callback_group=get_device_info_cb_group) self.wait_for_service_availability(self.get_revision_info_cli) self.get_logger().info(f"Create battery level service client: {BATTERY_LEVEL_SERVICE}") self.battery_level_cli = self.create_client(BatteryLevelSrv, BATTERY_LEVEL_SERVICE, callback_group=get_device_info_cb_group) self.wait_for_service_availability(self.battery_level_cli) self.get_logger().info(f"Create sensor status service client: {SENSOR_DATA_STATUS_SERVICE}") self.sensor_status_cli = self.create_client(SensorStatusCheckSrv, SENSOR_DATA_STATUS_SERVICE, callback_group=get_device_info_cb_group) self.wait_for_service_availability(self.sensor_status_cli) # Create a mutually exclusive callback group to set the tail light LED color values. set_led_color_cb_group = MutuallyExclusiveCallbackGroup() self.get_logger().info(f"Create set led color service client: {SET_CAR_LED_SERVICE}") self.set_led_color_cli = self.create_client(SetLedCtrlSrv, SET_CAR_LED_SERVICE, callback_group=set_led_color_cb_group) self.wait_for_service_availability(self.set_led_color_cli) # Create a reentrant callback group to set the tail light LED color values. get_led_color_cb_group = ReentrantCallbackGroup() self.get_logger().info(f"Create get led color service client: {GET_CAR_LED_SERVICE}") self.get_led_color_cli = self.create_client(GetLedCtrlSrv, GET_CAR_LED_SERVICE, callback_group=get_led_color_cb_group) self.wait_for_service_availability(self.get_led_color_cli) # Create a reentrant callback group to call load model services. model_load_cb_group = ReentrantCallbackGroup() self.get_logger().info(f"Create verify model ready service client: {VERIFY_MODEL_READY_SERVICE}") self.verify_model_ready_cli = self.create_client(VerifyModelReadySrv, VERIFY_MODEL_READY_SERVICE, callback_group=model_load_cb_group) self.wait_for_service_availability(self.verify_model_ready_cli) self.get_logger().info(f"Create configure LiDAR service client: {CONFIGURE_LIDAR_SERVICE}") self.configure_lidar_cli = self.create_client(LidarConfigSrv, CONFIGURE_LIDAR_SERVICE, callback_group=model_load_cb_group) self.wait_for_service_availability(self.configure_lidar_cli) self.get_logger().info(f"Create model state service client: {MODEL_STATE_SERVICE}") self.model_state_cli = self.create_client(ModelStateSrv, MODEL_STATE_SERVICE, callback_group=model_load_cb_group) self.wait_for_service_availability(self.model_state_cli) # Create a reentrant callback group to call model loading status service. is_model_loading_cb_group = ReentrantCallbackGroup() self.get_logger().info(f"Create is model loading service client: {IS_MODEL_LOADING_SERVICE}") self.is_model_loading_cli = self.create_client(GetModelLoadingStatusSrv, IS_MODEL_LOADING_SERVICE, callback_group=is_model_loading_cb_group) self.wait_for_service_availability(self.is_model_loading_cli) # Create a mutually exclusive callback group to call model action services. model_action_cb_group = MutuallyExclusiveCallbackGroup() self.get_logger().info(f"Create upload model service client: {CONSOLE_MODEL_ACTION_SERVICE}") self.model_action_cli = self.create_client(ConsoleModelActionSrv, CONSOLE_MODEL_ACTION_SERVICE, callback_group=model_action_cb_group) self.wait_for_service_availability(self.model_action_cli) # Create a reentrant callback group to call software update check service. sw_update_state_cb_group = ReentrantCallbackGroup() self.get_logger().info("Create sw update state check service client: " f"{SOFTWARE_UPDATE_CHECK_SERVICE_NAME}") self.sw_update_state_cli = self.create_client(SoftwareUpdateCheckSrv, SOFTWARE_UPDATE_CHECK_SERVICE_NAME, callback_group=sw_update_state_cb_group) self.wait_for_service_availability(self.sw_update_state_cli) # Create a reentrant callback group to call software update services. sw_update_cb_group = ReentrantCallbackGroup() self.get_logger().info(f"Create begin sw update service client: {BEGIN_UPDATE_SERVICE}") self.begin_sw_update_cli = self.create_client(BeginSoftwareUpdateSrv, BEGIN_UPDATE_SERVICE, callback_group=sw_update_cb_group) self.wait_for_service_availability(self.begin_sw_update_cli) self.get_logger().info("Create sw update status service client: " f"{SOFTWARE_UPDATE_CHECK_SERVICE_NAME}") self.sw_update_status_cli = self.create_client(SoftwareUpdateStateSrv, SOFTWARE_UPDATE_STATE_SERVICE, callback_group=sw_update_cb_group) self.wait_for_service_availability(self.sw_update_status_cli) # Create a reentrant exclusive callback group for the software progress subscriber. # Guaranteed delivery is needed to send messages to late-joining subscription. qos_profile = QoSProfile(depth=1) qos_profile.history = QoSHistoryPolicy.KEEP_LAST qos_profile.reliability = QoSReliabilityPolicy.RELIABLE self.sw_update_pct_sub_cb_group = ReentrantCallbackGroup() self.get_logger().info("Create sw update status service client: " f"{SOFTWARE_UPDATE_STATE_SERVICE}") self.sw_update_pct_sub = self.create_subscription(SoftwareUpdatePctMsg, SOFTWARE_UPDATE_PCT_TOPIC, self.sw_update_pct_sub_cb, callback_group=self.sw_update_pct_sub_cb_group, qos_profile=qos_profile) # Create a reentrant callback group to call autonomous throttle update service. set_throttle_cb_group = ReentrantCallbackGroup() self.get_logger().info("Create set throttle service client: " f"{AUTONOMOUS_THROTTLE_SERVICE}") self.set_throttle_cli = self.create_client(NavThrottleSrv, AUTONOMOUS_THROTTLE_SERVICE, callback_group=set_throttle_cb_group) self.wait_for_service_availability(self.set_throttle_cli) # Create a reentrant callback group to call Get Ctrl Modes service. get_ctrl_modes_cb_group = ReentrantCallbackGroup() self.get_logger().info("Create Get Ctrl Modes service client: " f"{GET_CTRL_MODES_SERVICE}") self.get_ctrl_modes_cli = self.create_client(GetCtrlModesSrv, GET_CTRL_MODES_SERVICE, callback_group=get_ctrl_modes_cb_group) self.wait_for_service_availability(self.get_ctrl_modes_cli) # Create a reentrant callback group to call otg link state service. otg_link_state_cb_group = ReentrantCallbackGroup() self.get_logger().info("Create otg link state service client: " f"{GET_OTG_LINK_STATE_SERVICE}") self.otg_link_state_cli = self.create_client(OTGLinkStateSrv, GET_OTG_LINK_STATE_SERVICE, callback_group=otg_link_state_cb_group) self.wait_for_service_availability(self.otg_link_state_cli) # Create a reentrant callback group to publish manual drive messages. manual_pub_drive_msg_cb_group = ReentrantCallbackGroup() self.get_logger().info(f"Create manual drive publisher: {MANUAL_DRIVE_TOPIC}") self.pub_manual_drive = self.create_publisher(ServoCtrlMsg, MANUAL_DRIVE_TOPIC, 1, callback_group=manual_pub_drive_msg_cb_group) # Create a reentrant callback group to publish calibration drive messages. cal_pub_drive_msg_cb_group = ReentrantCallbackGroup() self.get_logger().info(f"Create calibration drive publisher: {CAL_DRIVE_TOPIC}") self.pub_calibration_drive = self.create_publisher(ServoCtrlMsg, CAL_DRIVE_TOPIC, 1, callback_group=cal_pub_drive_msg_cb_group) # Double buffer object to safely updat the latest status and installation percentage # during software update setup. self.pct_dict_db = DoubleBuffer(clear_data_on_get=False) self.pct_dict_db.put({"status": "unknown", "update_pct": 0.0}) # Heartbeat timer. self.timer_count = 0 self.timer = self.create_timer(5.0, self.timer_callback)
#!/usr/bin/python3 import time import rclpy from gateway_interfaces.msg import SomeRequest, SomeResponse from rclpy.node import Node from rclpy.qos import QoSProfile, QoSDurabilityPolicy, QoSHistoryPolicy, QoSReliabilityPolicy _NODE_NAME = 'some_request_to_some_response' _REQUEST_TOPIC = 'some_request' _RESPONSE_TOPIC = 'some_response' _QOS_RESPONSE = QoSProfile(depth=1) _QOS_RESPONSE.durability = QoSDurabilityPolicy.SYSTEM_DEFAULT _QOS_RESPONSE.history = QoSHistoryPolicy.SYSTEM_DEFAULT _QOS_RESPONSE.reliability = QoSReliabilityPolicy.SYSTEM_DEFAULT _QOS_REQUEST = QoSProfile(depth=1) _QOS_REQUEST.durability = QoSDurabilityPolicy.SYSTEM_DEFAULT _QOS_REQUEST.history = QoSHistoryPolicy.SYSTEM_DEFAULT _QOS_REQUEST.reliability = QoSReliabilityPolicy.SYSTEM_DEFAULT _SOURCE_ID = 1 class SomeRequestToSomeResponseNode(Node): def __init__(self): super().__init__(_NODE_NAME) self.logger = self.get_logger()