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