diff --git a/docs/conf.py b/docs/conf.py index 72370be29..86a815dc2 100644 --- a/docs/conf.py +++ b/docs/conf.py @@ -11,6 +11,7 @@ # All configuration values have a default; values that are commented out # serve to show the default. +import datetime import os import os.path import sys @@ -88,6 +89,9 @@ # Add any paths that contain templates here, relative to this directory. templates_path = ["_templates"] +now = datetime.datetime.now().astimezone() +# Sun, Jan 30th, 2024 11:59 PM (EST) +html_last_updated_fmt = now.strftime("%a, %b %d, %Y %I:%M %p (%Z)") # The suffix(es) of source filenames. # You can specify multiple suffix as a list of string: @@ -102,7 +106,7 @@ # General information about the project. project = "mil" -copyright = "2019-2022, Machine Intelligence Lab" +copyright = "2019-2024, Machine Intelligence Lab" author = "Machine Intelligence Lab" # The version info for the project you're documenting, acts as replacement for diff --git a/docs/reference/axros/examples.rst b/docs/reference/axros/examples.rst index a44ab3592..94484b6a7 100644 --- a/docs/reference/axros/examples.rst +++ b/docs/reference/axros/examples.rst @@ -1,24 +1,24 @@ Examples ^^^^^^^^ -The following example starts a sublisher and a subscriber which listens to the messages -sublished by the sublisher. After two seconds, it reports how many messages it was +The following example starts a publisher and a subscriber which listens to the messages +published by the publisher. After two seconds, it reports how many messages it was able to successfully receive. .. code-block:: python import asyncio from rosgraph_msgs.msg import Clock - from axros import NodeHandle, sublisher, Subscriber + from axros import NodeHandle, Publisher, Subscriber import uvloop - async def subsub(nh: NodeHandle, sub: sublisher, sub: Subscriber): + async def pubsub(nh: NodeHandle, pub: Publisher, sub: Subscriber): count = 0 try: while True: await asyncio.sleep(0) msg = (Clock(nh.get_time())) - sub.sublish(msg) + pub.publish(msg) recv = await sub.get_next_message() if (recv == msg): count += 1 @@ -30,13 +30,13 @@ able to successfully receive. async def main(): nh = await NodeHandle.from_argv("node", "", anonymous = True) async with nh: - sub = nh.advertise('clock2', Clock, latching = True) + pub = nh.advertise('clock2', Clock, latching = True) sub = nh.subscribe('clock2', Clock) - await asyncio.gather(sub.setup(), sub.setup()) - subsub_task = asyncio.create_task(subsub(nh, sub, sub)) + await asyncio.gather(pub.setup(), sub.setup()) + pubsub_task = asyncio.create_task(pubsub(nh, pub, sub)) print("Everything is setup. Waiting two seconds.") await asyncio.sleep(2) - subsub_task.cancel() + pubsub_task.cancel() if __name__ == "__main__": uvloop.install() @@ -62,8 +62,8 @@ The node handle can be used for general purposes, such as parameters and sleepin This parameter does not exist! >>> await nh.sleep(2) # Sleeps for 2 seconds -The node handle can also be used for sublishing and subscribing to topics. Note -that all sublishers and subscribers must be setup. +The node handle can also be used for publishing and subscribing to topics. Note +that all Publishers and subscribers must be setup. .. code-block:: python @@ -71,19 +71,19 @@ that all sublishers and subscribers must be setup. >>> import axros >>> nh = await axros.NodeHandle("/test", "special_node", "localhost", os.environ["ROS_MASTER_URI"], {}) >>> from std_msgs.msg import Int32 - >>> sub = nh.advertise("running_time", Int32) - >>> await sub.setup() - >>> async def sublish(): + >>> pub = nh.advertise("running_time", Int32) + >>> await pub.setup() + >>> async def publish(): ... try: ... count = 0 ... while True: - ... sub.sublish(Int32(count)) + ... pub.publish(Int32(count)) ... count += 1 ... await asyncio.sleep(1) ... except asyncio.CancelledError as _: - ... # When task gets cancelled, stop sublishing + ... # When task gets cancelled, stop publishing ... pass - >>> task = asyncio.create_task(sublish()) # Start sublishing! + >>> task = asyncio.create_task(publish()) # Start publishing! >>> sub = nh.subscribe("running_time", Int32) >>> await sub.setup() >>> while True: diff --git a/docs/reference/bagging.rst b/docs/reference/bagging.rst new file mode 100644 index 000000000..feb007dad --- /dev/null +++ b/docs/reference/bagging.rst @@ -0,0 +1,9 @@ +Online Bagger +------------- + +.. currentmodule:: mil_tools + +.. attributetable:: nodes.online_bagger.OnlineBagger + +.. autoclass:: nodes.online_bagger.OnlineBagger + :members: diff --git a/docs/reference/index.rst b/docs/reference/index.rst index b6f96f49e..c63581a0c 100644 --- a/docs/reference/index.rst +++ b/docs/reference/index.rst @@ -31,3 +31,4 @@ by MIL. These subsystems relate to a variety of processes. poi pneumatic sabertooth + bagging diff --git a/docs/software/alarms.md b/docs/software/alarms.md new file mode 100644 index 000000000..80fb51485 --- /dev/null +++ b/docs/software/alarms.md @@ -0,0 +1,114 @@ +# Alarms in ROS + +In the realm of building dependable control systems, the importance of error detection +and effective error-handling mechanisms cannot be overstated. Within this context, +MIL presents a robust solution in the form of a live alarm system. This alarm system +operates discreetly in the background of both the robot's mission and driver codebases, +ready to be activated upon the emergence of errors. Notably, the alarm code doesn't +solely serve to identify and address errors; it can also adeptly manage changes +or updates that extend beyond error scenarios. + +## ROS Alarms: A Service-Oriented Architecture + +The architecture of ROS alarms distinguishes itself by employing a service-oriented +model rather than the usual topic-based approach. In ROS, Services act as the +conduits for interaction between nodes, functioning in a request-response manner. +While ROS topics enable asynchronous data exchange, services facilitate nodes in +seeking specific actions or information from other nodes, awaiting a subsequent +response before proceeding. This method of waiting before proceeding is known as a +synchronous data exchange. This proves especially valuable in tasks that require +direct engagement, such as data retrieval or computations. + +## Alarm System Logic + +The alarm system's functionality is more intricate than that of a typical ROS +service, which usually manages operations of base types (ints, strings, etc.). +In this scenario, the alarm's service server is engineered to manage the tasks +of updating, querying, and processing an alarm object. ROS alarms encompass two +distinct types of clients: the alarm broadcaster and the alarm listener. The +broadcaster initializes and triggers alarms in response to errors or changes, +while the listener monitors the broadcaster's activity and activates designated +a callback function when alarms are raised. The callback function should handle +the error or change appropriately. + +To successfully leverage alarms, the initialization of both the broadcaster and +listener is needed. The listener should be configured to execute a predefined +callback function, addressing errors or changes detected by the broadcaster. +Within your codebase, error detection and alarm-raising procedures should be +integrated. If orchestrated correctly, the callback function will be automatically +invoked, underscoring successful error mitigation. + +Note that there are several special properties that can be attached to your alarm. +Here are a couple of examples: +* When you raise an alarm you can assign a severity level to the alarm [0, 5]. +* You can attach multiple callback functions to the alarm. + * **This is where severity comes into play!** By specifying the required + severity level that is needed to execute the callback when initializing the + function, you can choose which callbacks are executed when the alarm is raised. + * You can also specify a range of severity levels that the alarm would need to + execute a given callback. + +Here is a line-by-line breakdown of an example alarm implementation: + +```python +ab = AlarmBroadcaster("test_alarm") +al = AlarmListener("test_alarm") +ab.clear_alarm() +rospy.sleep(0.1) +``` +This is how you would initialize the alarm broadcaster and listener. Here +make sure to clear any previous alarm data in the broadcaster. + +```python +al.add_callback(cb1) +``` +Make sure to establish the callback function that should be executed once +the alarm is activated. + +```python +ab.raise_alarm() +rospy.sleep(0.1) +assert al.is_raised() +assert cb1_ran +``` +When the alarm is sounded via the `raise_alarm()` function, the callback will be +executed automatically. + +```python +al.clear_callbacks() + +al.add_callback(cb1, severity_required=2) +al.add_callback(cb2, call_when_raised=False) + +rospy.loginfo("Severity Range Test 1") +ab.raise_alarm(severity=4) +rospy.sleep(0.1) +assert not cb1_ran +assert cb2_ran +cb2_ran = False + +rospy.loginfo("Severity Range Test 2") +ab.raise_alarm(severity=1) +rospy.sleep(0.1) +assert cb1_ran +assert not cb2_ran +cb1_ran = False +``` +Note that you can also attach some special properties to your alarm. For instance, +you can attach multiple callback functions to the alarm. You can also configure +whether the callback function should be automatically executed when the alarm is +raised or whether it should be executed manually. Finally, you can assign a +severity level to the alarm which can tell the alarm code which callback functions +should be run. + +## Applications and Context + +The applications of ROS alarms span various contexts, with one notable application +residing in the control of the submersible vehicle's thrust and killboard. The +thrust and killboard, responsible for the sub's electronic operations, is +integrally associated with ROS alarms. Upon the board's activation or deactivation +(hard or soft kill), alarms are invoked to apprise users of these changes. The +listener's callback function comes into play, ensuring that alarms are updated +in alignment with the board's current state. This process triggered each time +the board is deactivated, creates a system whereby users are continually informed +about the board's status changes – essentially manifesting a dynamic live alarm system. diff --git a/docs/software/index.rst b/docs/software/index.rst index 9e779a0eb..299168e7c 100644 --- a/docs/software/index.rst +++ b/docs/software/index.rst @@ -15,6 +15,8 @@ Various documentation related to practices followed by the MIL Software team. asyncio rqt rostest + alarms + preflight Bash Style Guide C++ Style Guide Python Style Guide diff --git a/docs/software/preflight.md b/docs/software/preflight.md new file mode 100644 index 000000000..a2a088fb3 --- /dev/null +++ b/docs/software/preflight.md @@ -0,0 +1,2 @@ +```{include} ../../mil_common/utils/mil_tools/scripts/mil-preflight/preflight.md +``` diff --git a/docs/subjugator/reference.rst b/docs/subjugator/reference.rst index 2ed8e1296..4b5433071 100644 --- a/docs/subjugator/reference.rst +++ b/docs/subjugator/reference.rst @@ -100,6 +100,11 @@ ActuatorBoardSimulation .. autoclass:: sub_actuator_board.ActuatorBoardSimulation :members: +ActuatorPacketId +^^^^^^^^^^^^^^^^ +.. autoclass:: sub_actuator_board.ActuatorPacketId + :members: + ActuatorSetPacket ^^^^^^^^^^^^^^^^^ .. attributetable:: sub_actuator_board.ActuatorSetPacket @@ -183,6 +188,13 @@ HeartbeatReceivePacket .. autoclass:: sub9_thrust_and_kill_board.HeartbeatReceivePacket :members: +ThrusterId +^^^^^^^^^^ +.. attributetable:: sub9_thrust_and_kill_board.ThrusterId + +.. autoclass:: sub9_thrust_and_kill_board.ThrusterId + :members: + ThrustSetPacket ^^^^^^^^^^^^^^^ .. attributetable:: sub9_thrust_and_kill_board.ThrustSetPacket diff --git a/mil_common/axros b/mil_common/axros index a4951d333..8cdf13186 160000 --- a/mil_common/axros +++ b/mil_common/axros @@ -1 +1 @@ -Subproject commit a4951d33356c349856532ddef5c97c6ec6810c28 +Subproject commit 8cdf131866af08edf0bbe3ac9020e1da47b3e432 diff --git a/mil_common/drivers/mil_pneumatic_actuator/mil_pneumatic_actuator/board.py b/mil_common/drivers/mil_pneumatic_actuator/mil_pneumatic_actuator/board.py index 0a9217691..bf1c13f21 100644 --- a/mil_common/drivers/mil_pneumatic_actuator/mil_pneumatic_actuator/board.py +++ b/mil_common/drivers/mil_pneumatic_actuator/mil_pneumatic_actuator/board.py @@ -31,10 +31,7 @@ class PnuematicActuatorDriverChecksumError(PnuematicActuatorDriverError): """ def __init__(self, checksum_is, checksum_should_be): - message = "Invalid checksum. Recievied {}, should be {}".format( - hex(checksum_is), - hex(checksum_should_be), - ) + message = f"Invalid checksum. Recievied {hex(checksum_is)}, should be {hex(checksum_should_be)}" super().__init__(message) @@ -46,9 +43,8 @@ class PnuematicActuatorDriverResponseError(PnuematicActuatorDriverError): """ def __init__(self, received, expected): - message = "Unexpected response. Expected {}, received {}".format( - hex(received), - hex(expected), + message = ( + f"Unexpected response. Expected {hex(received)}, received {hex(expected)}" ) super().__init__(message) diff --git a/mil_common/drivers/mil_usb_to_can/mil_usb_to_can/sub8/application_packet.py b/mil_common/drivers/mil_usb_to_can/mil_usb_to_can/sub8/application_packet.py index 904867cfa..a1995f6d5 100644 --- a/mil_common/drivers/mil_usb_to_can/mil_usb_to_can/sub8/application_packet.py +++ b/mil_common/drivers/mil_usb_to_can/mil_usb_to_can/sub8/application_packet.py @@ -116,11 +116,7 @@ def from_bytes( return packet def __repr__(self): - return "{}(identifier={}, payload={})".format( - self.__class__.__name__, - self.identifier, - self.payload, - ) + return f"{self.__class__.__name__}(identifier={self.identifier}, payload={self.payload})" def __eq__(self, other): if not isinstance(other, ApplicationPacket): diff --git a/mil_common/drivers/mil_usb_to_can/mil_usb_to_can/sub8/utils.py b/mil_common/drivers/mil_usb_to_can/mil_usb_to_can/sub8/utils.py index 7524e6dfe..2acd3df1a 100644 --- a/mil_common/drivers/mil_usb_to_can/mil_usb_to_can/sub8/utils.py +++ b/mil_common/drivers/mil_usb_to_can/mil_usb_to_can/sub8/utils.py @@ -486,8 +486,4 @@ def __bytes__(self) -> bytes: return data def __str__(self): - return "CommandPacket(filter_id={}, is_receive={}, receive_length={})".format( - self.filter_id, - self.is_receive, - self.length, - ) + return f"CommandPacket(filter_id={self.filter_id}, is_receive={self.is_receive}, receive_length={self.length})" diff --git a/mil_common/drivers/mil_usb_to_can/mil_usb_to_can/sub9/packet.py b/mil_common/drivers/mil_usb_to_can/mil_usb_to_can/sub9/packet.py index 59ddc3a72..8b123bfc9 100644 --- a/mil_common/drivers/mil_usb_to_can/mil_usb_to_can/sub9/packet.py +++ b/mil_common/drivers/mil_usb_to_can/mil_usb_to_can/sub9/packet.py @@ -6,8 +6,8 @@ from functools import lru_cache from typing import ClassVar, get_type_hints -SYNC_CHAR_1 = ord("3") -SYNC_CHAR_2 = ord("7") +SYNC_CHAR_1 = 0x37 +SYNC_CHAR_2 = 0x01 _packet_registry: dict[int, dict[int, type[Packet]]] = {} @@ -117,7 +117,7 @@ def _calculate_checksum(cls, data: bytes) -> tuple[int, int]: def __bytes__(self): payload = struct.pack(self.payload_format, *self.__dict__.values()) data = struct.pack( - f"BBBBH{len(payload)}s", + f" Packet: @@ -141,12 +141,12 @@ def from_bytes(cls, packed: bytes) -> Packet: if msg_id in _packet_registry and subclass_id in _packet_registry[msg_id]: subclass = _packet_registry[msg_id][subclass_id] payload = packed[6:-2] - if struct.unpack("BB", packed[-2:]) != cls._calculate_checksum( + if struct.unpack(" packet_two + def _pack_checksum(self, byte_string: bytes) -> int: + checksum = Packet._calculate_checksum(byte_string) + return int.from_bytes(struct.pack("3} {r[1]:>3} {r[2]:>3}]".format( - l=images_and_labels[i][1], - r=images_and_labels[i][0][mouse_y, mouse_x], - ) - ) + text = f"[{images_and_labels[i][1][0]}, {images_and_labels[i][1][1]}, {images_and_labels[i][1][2]}] = [{images_and_labels[i][0][mouse_y, mouse_x][0]:>3} {images_and_labels[i][0][mouse_y, mouse_x][1]:>3} {images_and_labels[i][0][mouse_y, mouse_x][2]:>3}]" (t_w, t_h), _ = cv2.getTextSize( text, cv2.FONT_HERSHEY_PLAIN, diff --git a/mil_common/perception/mil_vision/src/mil_vision_lib/active_contours.cpp b/mil_common/perception/mil_vision/src/mil_vision_lib/active_contours.cpp index 46d902eee..e01a3d074 100644 --- a/mil_common/perception/mil_vision/src/mil_vision_lib/active_contours.cpp +++ b/mil_common/perception/mil_vision/src/mil_vision_lib/active_contours.cpp @@ -37,7 +37,7 @@ void initPerturbationCache() auto in = seq_to_mod_border_idxs[entry]; auto out = seq_to_mod_border_idxs[exit]; growRoute(perturbation_list, occupied_squares, in, out); // Appends all possible paths from entry point to - } // exit point at the corresponding cache position + } // exit point at the corresponding cache position } } @@ -97,9 +97,8 @@ vector getHoodIdxs(uint8_t idx, bool include_border) hood.begin(), remove_if(hood.begin(), hood.end(), [](uint8_t x) { return x < 0 || x >= 39 || x % 8 > 4; })); else hood = vector( - hood.begin(), - remove_if(hood.begin(), hood.end(), - [](uint8_t x) { return x < 0 || x >= 39 || x / 8 == 0 || x / 8 == 4 || x % 8 == 0 || x % 8 > 3; })); + hood.begin(), remove_if(hood.begin(), hood.end(), [](uint8_t x) + { return x < 0 || x >= 39 || x / 8 == 0 || x / 8 == 4 || x % 8 == 0 || x % 8 > 3; })); return hood; } @@ -122,8 +121,7 @@ void growRoute(const vector& partial, const vector& occupied, // remove candidates that were neighbors of prev tail candidates = vector(candidates.begin(), - remove_if(candidates.begin(), candidates.end(), - [&occupied](uint8_t x) + remove_if(candidates.begin(), candidates.end(), [&occupied](uint8_t x) { return find(occupied.begin(), occupied.end(), x) != occupied.end(); })); auto next_occupied = occupied; // neighbors of current tailed should be blacklisted for next route growth @@ -226,9 +224,8 @@ bool ClosedCurve::validateCurve(std::vector& curve) { if (count > 1) { - auto conflict_pt = - find_if(forbidden_neighbors.begin(), forbidden_neighbors.end(), - [pt](Point2i test_pt) { return mil_vision::Perturbations::isNeighborPoint(pt, test_pt); }); + auto conflict_pt = find_if(forbidden_neighbors.begin(), forbidden_neighbors.end(), [pt](Point2i test_pt) + { return mil_vision::Perturbations::isNeighborPoint(pt, test_pt); }); cout << "failure pts: " << curve[1] << "\t" << (conflict_pt != forbidden_neighbors.end() ? Point2i(0, 0) : *conflict_pt) << endl; return false; @@ -238,9 +235,8 @@ bool ClosedCurve::validateCurve(std::vector& curve) { if (count > 0) { - auto conflict_pt = - find_if(forbidden_neighbors.begin(), forbidden_neighbors.end(), - [pt](Point2i test_pt) { return mil_vision::Perturbations::isNeighborPoint(pt, test_pt); }); + auto conflict_pt = find_if(forbidden_neighbors.begin(), forbidden_neighbors.end(), [pt](Point2i test_pt) + { return mil_vision::Perturbations::isNeighborPoint(pt, test_pt); }); cout << "failure pts: " << curve[1] << "\t" << (conflict_pt != forbidden_neighbors.end() ? Point2i(0, 0) : *conflict_pt) << endl; return false; diff --git a/mil_common/perception/mil_vision/src/mil_vision_lib/cv_utils.cc b/mil_common/perception/mil_vision/src/mil_vision_lib/cv_utils.cc index 750435fcb..7d670a0d3 100644 --- a/mil_common/perception/mil_vision/src/mil_vision_lib/cv_utils.cc +++ b/mil_common/perception/mil_vision/src/mil_vision_lib/cv_utils.cc @@ -71,9 +71,7 @@ std::vector generate_gaussian_kernel_1D(size_t kernel_size, float sigma) std::vector find_local_maxima(const cv::MatND &histogram, float thresh_multiplier) { std::stringstream ros_log; - ros_log << "\x1b[1;31m" - << "find_local_maxima" - << "\x1b[0m" << std::endl; + ros_log << "\x1b[1;31m" << "find_local_maxima" << "\x1b[0m" << std::endl; std::vector local_maxima, threshed_local_maxima; float global_maximum = -std::numeric_limits::infinity(); @@ -121,9 +119,7 @@ std::vector find_local_maxima(const cv::MatND &histogram, float thres std::vector find_local_minima(const cv::MatND &histogram, float thresh_multiplier) { std::stringstream ros_log; - ros_log << "\x1b[1;31m" - << "find_local_minima" - << "\x1b[0m" << std::endl; + ros_log << "\x1b[1;31m" << "find_local_minima" << "\x1b[0m" << std::endl; std::vector local_minima, threshed_local_minima; float global_minimum = std::numeric_limits::infinity(); @@ -172,9 +168,7 @@ std::vector find_local_minima(const cv::MatND &histogram, float thres unsigned int select_hist_mode(std::vector &histogram_modes, int target) { std::stringstream ros_log; - ros_log << "\x1b[1;31m" - << "select_hist_mode" - << "\x1b[0m" << std::endl; + ros_log << "\x1b[1;31m" << "select_hist_mode" << "\x1b[0m" << std::endl; std::vector distances; BOOST_FOREACH (cv::Point mode, histogram_modes) @@ -202,9 +196,7 @@ void statistical_image_segmentation(const cv::Mat &src, cv::Mat &dest, cv::Mat & const float sigma, const float low_thresh_gain, const float high_thresh_gain) { std::stringstream ros_log; - ros_log << "\x1b[1;31m" - << "statistical_image_segmentation" - << "\x1b[0m" << std::endl; + ros_log << "\x1b[1;31m" << "statistical_image_segmentation" << "\x1b[0m" << std::endl; // Calculate histogram cv::MatND hist, hist_smooth, hist_derivative; @@ -559,10 +551,8 @@ void FrameHistory::image_callback(const sensor_msgs::ImageConstPtr &image_msg, ImageWithCameraInfo current_frame(image_msg, info_msg); bool full = _frame_history_ring_buffer.size() >= history_size; std::stringstream debug_msg; - debug_msg << "Adding frame to ring buffer " - << "[frame=" << frame_count << "," - << "full=" << (full ? "true" : "false") << ",frames_available=" << _frame_history_ring_buffer.size() << "]" - << std::endl; + debug_msg << "Adding frame to ring buffer " << "[frame=" << frame_count << "," << "full=" << (full ? "true" : "false") + << ",frames_available=" << _frame_history_ring_buffer.size() << "]" << std::endl; ROS_DEBUG(debug_msg.str().c_str()); if (!full) { diff --git a/mil_common/ros_alarms/nodes/alarm_server.py b/mil_common/ros_alarms/nodes/alarm_server.py index b3a023778..838d543be 100755 --- a/mil_common/ros_alarms/nodes/alarm_server.py +++ b/mil_common/ros_alarms/nodes/alarm_server.py @@ -134,9 +134,7 @@ def _handle_meta_alarm(self, meta_alarm, sub_alarms): alarm.problem_description = "Raised by meta alarm" else: rospy.logwarn( - "Meta alarm callback for {} failed to return an Alarm or boolean".format( - meta_alarm, - ), + f"Meta alarm callback for {meta_alarm} failed to return an Alarm or boolean", ) return self.set_alarm(alarm) diff --git a/mil_common/utils/mil_poi/CMakeLists.txt b/mil_common/utils/mil_poi/CMakeLists.txt index 8532c7d71..66989bd8d 100644 --- a/mil_common/utils/mil_poi/CMakeLists.txt +++ b/mil_common/utils/mil_poi/CMakeLists.txt @@ -29,3 +29,8 @@ generate_messages( ) catkin_package() + +if(CATKIN_ENABLE_TESTING) + find_package(rostest REQUIRED) + add_rostest(test/poi.test) +endif() diff --git a/mil_common/utils/mil_poi/mil_poi/server.py b/mil_common/utils/mil_poi/mil_poi/server.py index bf7fee2cd..1bad17210 100644 --- a/mil_common/utils/mil_poi/mil_poi/server.py +++ b/mil_common/utils/mil_poi/mil_poi/server.py @@ -119,11 +119,7 @@ def transform_position(self, ps: PointStamped) -> Optional[Point]: return ps_tf.point except tf2_ros.TransformException as e: rospy.logwarn( - 'Error transforming "{}" to "{}": {}'.format( - ps.header.frame_id, - self.global_frame, - e, - ), + f'Error transforming "{ps.header.frame_id}" to "{self.global_frame}": {e}', ) return None diff --git a/mil_common/utils/mil_poi/test/poi.test b/mil_common/utils/mil_poi/test/poi.test new file mode 100644 index 000000000..fa6be65ba --- /dev/null +++ b/mil_common/utils/mil_poi/test/poi.test @@ -0,0 +1,6 @@ + + + + + + diff --git a/mil_common/utils/mil_poi/test/test_async.py b/mil_common/utils/mil_poi/test/test_async.py new file mode 100755 index 000000000..4a4798d3e --- /dev/null +++ b/mil_common/utils/mil_poi/test/test_async.py @@ -0,0 +1,98 @@ +#!/usr/bin/env python3 + +import asyncio +import unittest + +import axros +import rostest +from geometry_msgs.msg import Point, PointStamped +from mil_poi.srv import ( + AddPOI, + AddPOIRequest, + DeletePOI, + DeletePOIRequest, + MovePOI, + MovePOIRequest, +) +from std_msgs.msg import Header + + +class POITestAsync(unittest.IsolatedAsyncioTestCase): + async def asyncSetUp(self): + self.nh = axros.NodeHandle.from_argv("basic", always_default_name=True) + await self.nh.setup() + self.add_service = self.nh.get_service_client("/poi_server/add", AddPOI) + self.move_service = self.nh.get_service_client("/poi_server/move", MovePOI) + self.delete_service = self.nh.get_service_client( + "/poi_server/delete", + DeletePOI, + ) + await asyncio.gather( + self.add_service.wait_for_service(), + self.move_service.wait_for_service(), + self.delete_service.wait_for_service(), + ) + + async def test_points(self): + # Adding + test = await self.add_service( + AddPOIRequest( + name="test", + position=PointStamped(header=Header(), point=Point(0.0, 1.0, 2.0)), + ), + ) + self.assertTrue(test.success) + test = await self.add_service( + AddPOIRequest( + name="test2", + position=PointStamped(header=Header(), point=Point(0.0, 1.0, 2.0)), + ), + ) + self.assertTrue(test.success) + + # Moving + test = await self.move_service( + MovePOIRequest( + name="test", + position=PointStamped(header=Header(), point=Point(0.0, -1.0, -2.0)), + ), + ) + self.assertTrue(test.success) + test = await self.move_service( + MovePOIRequest( + name="test2", + position=PointStamped(header=Header(), point=Point(0.0, 0.0, 0.0)), + ), + ) + self.assertTrue(test.success) + + # Moving a non-existent POI should return False + test = await self.move_service( + MovePOIRequest( + name="test3", + position=PointStamped(header=Header(), point=Point(0.0, 0.0, 0.0)), + ), + ) + self.assertFalse(test.success) + + # Deleting + test = await self.delete_service(DeletePOIRequest(name="test")) + self.assertTrue(test.success) + test = await self.delete_service(DeletePOIRequest(name="test2")) + self.assertTrue(test.success) + + # Deleting a non-existent POI should return False + test = await self.delete_service(DeletePOIRequest(name="test")) + self.assertFalse(test.success) + + # Deleting a non-existent POI should return False + test = await self.delete_service(DeletePOIRequest(name="test3")) + self.assertFalse(test.success) + + async def asyncTearDown(self): + await self.nh.shutdown() + + +if __name__ == "__main__": + rostest.rosrun("mil_poi", "test_poi", POITestAsync) + unittest.main() diff --git a/mil_common/utils/mil_poi/test/test_sync.py b/mil_common/utils/mil_poi/test/test_sync.py new file mode 100755 index 000000000..661153e4d --- /dev/null +++ b/mil_common/utils/mil_poi/test/test_sync.py @@ -0,0 +1,124 @@ +#!/usr/bin/env python3 +import random +import string +import unittest + +import genpy +import rospy +import rostest +from geometry_msgs.msg import Point, PointStamped +from mil_poi.srv import ( + AddPOI, + DeletePOI, + MovePOI, + MovePOIRequest, +) +from std_msgs.msg import Header + + +class POITest(unittest.TestCase): + def setUp(self): + # make one poi here + rospy.init_node("poi_test_node") + self.poi_name = "test_poi" + self.poi_position = PointStamped(header=Header(), point=Point(0.0, 1.0, 2.0)) + self.add_poi() + + def add_poi(self): + rospy.wait_for_service("/poi_server/add") + service = rospy.ServiceProxy("/poi_server/add", AddPOI) + response = service(self.poi_name, self.poi_position) + self.assertTrue(response.success) + + def test_add(self): + # Call the add_poi function to add a POI + self.add_poi() + + def test_move(self): + # Call the add_poi function to add a POI + self.add_poi() + + # Wait for the move_poi service to become available + rospy.wait_for_service("/poi_server/move") + move_service = rospy.ServiceProxy("/poi_server/move", MovePOI) + + # Move the POI to a new position + new_position = [1.0, 2.0, 3.0] # New position coordinates + move_response = move_service( + MovePOIRequest( + name=self.poi_name, + position=PointStamped(point=Point(*new_position)), + ), + ) + # Check if the additions were unsuccessful + self.assertTrue(move_response.success, f"Failed to move POI '{self.poi_name}'") + + def test_delete(self): + # Call the add_poi function to add a POI + self.add_poi() + + # Wait for the remove_poi service to become available + rospy.wait_for_service("/poi_server/delete") + remove_service = rospy.ServiceProxy("/poi_server/delete", DeletePOI) + + # Remove the added POI + remove_response = remove_service(self.poi_name) + + # Check if the removal was successful + self.assertTrue( + remove_response.success, + f"Failed to delete POI '{self.poi_name}'", + ) + + def test_long_string(self): + rospy.wait_for_service("/poi_server/add") + service = rospy.ServiceProxy("/poi_server/add", AddPOI) + + # Create a long string for the POI name + long_string = "".join(random.choices(string.ascii_letters, k=20000)) + + # Call the service to add a new POI with the long string name + response = service( + long_string, + PointStamped(header=Header(), point=Point(0.0, 1.0, 2.0)), + ) + self.assertTrue(response.success, response.message) + + def test_wrong_types(self): + # Wait for the add_poi service to become available + rospy.wait_for_service("/poi_server/add") + service = rospy.ServiceProxy("/poi_server/add", AddPOI) + + # Try adding a POI with wrong types of arguments + with self.assertRaises(genpy.message.SerializationError): + service(12321, [0.0, 2.3, 21.3]) # Incorrect name type + service("tester", ["hi", "wrong", "bad"]) # Incorrect position type + + def test_delete_twice(self): + # Call the add_poi function to add a POI + self.add_poi() + + # Wait for the remove_poi service to become available + rospy.wait_for_service("/poi_server/delete") + remove_service = rospy.ServiceProxy("/poi_server/delete", DeletePOI) + + # Remove the added POI + remove_response1 = remove_service(self.poi_name) + + # Try removing the same POI again + remove_response2 = remove_service(self.poi_name) + + # Check if the first removal was successful and the second removal was unsuccessful + self.assertTrue( + remove_response1.success, + f"Failed to remove POI '{self.poi_name}'", + ) + self.assertFalse( + remove_response2.success, + f"Removed POI '{self.poi_name}' twice", + ) + + +if __name__ == "__main__": + rostest.rosrun("mil_poi", "test_poi", POITest) + unittest.main() diff --git a/mil_common/utils/mil_tools/mil_ros_tools/label_me_bag.py b/mil_common/utils/mil_tools/mil_ros_tools/label_me_bag.py index aa7f91142..db63542f1 100755 --- a/mil_common/utils/mil_tools/mil_ros_tools/label_me_bag.py +++ b/mil_common/utils/mil_tools/mil_ros_tools/label_me_bag.py @@ -267,11 +267,7 @@ def completion_report(self): ) else: print( - "{}/{} TOTAL images labeled ({:.1%})".format( - total_xml_count, - total_img_count, - total_xml_count / total_img_count, - ), + f"{total_xml_count}/{total_img_count} TOTAL images labeled ({total_xml_count / total_img_count:.1%})", ) def _completion_bag(self, bag): diff --git a/mil_common/utils/mil_tools/nodes/online_bagger.py b/mil_common/utils/mil_tools/nodes/online_bagger.py index 5a0c6bb32..14c9ff178 100755 --- a/mil_common/utils/mil_tools/nodes/online_bagger.py +++ b/mil_common/utils/mil_tools/nodes/online_bagger.py @@ -1,11 +1,13 @@ #!/usr/bin/env python3 +from __future__ import annotations import argparse import datetime import itertools import os from collections import deque +from typing import TYPE_CHECKING import rosbag import rospy @@ -19,20 +21,30 @@ ) from tqdm import tqdm -""" -Online Bagger is a node which subscribes to a list of ros topics, -maintaining a buffer of the most recent n seconds. Parts or all of -these buffered topics can be written to a bag file by -sending a new goal to the /online_bagger/bag action server. - -When run with the -c flag, instead runs an action client which connects -to online bagger, triggering a bag write and displaying a progress bar -as it writes. - -""" +if TYPE_CHECKING: + import genpy class OnlineBagger: + """ + Node that maintains a list of bagged information relating to the specified + topics. + + Subscribes to a list of ROS topics, and maintains a buffer of the most recent + n seconds. Parts or all of these buffered topics can be written to a bag + file by sending a new goal to the /online_bagger/bag action server. When + run with the -c flag, instead runs an action client which connects to online + bagger, triggering a bag write and displaying a progress bar as it writes. + + Attributes: + successful_subscription_count (int): The number of successful subscriptions + to topics. + iteration_count (int): The number of iterations. + streaming (bool): Indicates whether the bagger is streaming. + subscriber_list (list[str]): The list of topics subscribed to the + OnlineBagger. + """ + BAG_TOPIC = "/online_bagger/bag" def __init__(self): @@ -67,6 +79,15 @@ def get_subscriber_list(self, status): """ Get string of all topics, if their subscribe status matches the input (True / False) Outputs each topics: time_buffer(float in seconds), subscribe_statue(bool), topic(string) + + Args: + status (bool): The subscription status used to search for topics with a matching + subscription status. + + Returns: + string: The list of topics that match the desired subscribe status. Each + line in the list contains the buffer time (in seconds) of the topic, the subscrition + status of the topic, and the topic name. """ sub_list = "" for topic in self.subscriber_list: @@ -99,10 +120,23 @@ def get_params(self): self.subscriber_list[topic[0]] = (time, False) def add_unique_topic(topic): + """ + Adds a topic to the subscriber list if the topic is not already in the + list. + + Args: + topic (str): The name of the topic to add to the subscriber list. + """ if topic not in self.subscriber_list: self.subscriber_list[topic] = (self.stream_time, False) def add_env_var(var): + """ + Adds topic(s) to the subscriber list. + + Args: + var (str): The topic(s) to add to the subscriber list. + """ for topic in var.split(): add_unique_topic(topic) @@ -178,18 +212,14 @@ def subscribe_loop(self): if i % 1000 == 0: rospy.logdebug("still subscribing!") rospy.loginfo( - "Subscribed to {} of {} topics, will try again every {} seconds".format( - self.successful_subscription_count, - len(self.subscriber_list), - self.resubscribe_period, - ), + f"Subscribed to {self.successful_subscription_count} of {len(self.subscriber_list)} topics, will try again every {self.resubscribe_period} seconds", ) self.resubscriber = rospy.Timer( rospy.Duration(self.resubscribe_period), self.subscribe, ) - def subscribe(self, time_info=None): + def subscribe(self, _: rospy.timer.TimerEvent | None = None): """ Subscribe to the topics defined in the yaml configuration file @@ -200,13 +230,12 @@ def subscribe(self, time_info=None): Each element in self.subscriber list is a list [topic, Bool] where the Bool tells the current status of the subscriber (success/failure). - - Return number of topics that failed subscription """ - if self.successful_subscription_count == len(self.subscriber_list): - if self.resubscriber is not None: - self.resubscriber.shutdown() - rospy.loginfo("All topics subscribed too! Shutting down resubscriber") + if (self.successful_subscription_count == len(self.subscriber_list)) and ( + self.resubscriber is not None + ): + self.resubscriber.shutdown() + rospy.loginfo("All topics subscribed too! Shutting down resubscriber") for topic, (time, subscribed) in self.subscriber_list.items(): if not subscribed: @@ -221,18 +250,29 @@ def subscribe(self, time_info=None): self.subscriber_list[topic] = (time, True) - def get_topic_duration(self, topic): - """ - Return current time duration of topic + def get_topic_duration(self, topic: str): """ + Returns the current time duration of topic + + Args: + topic (str): The topic for which the duration will be calculated. + Returns: + Duration: The time duration of the topic. + """ return self.topic_messages[topic][-1][0] - self.topic_messages[topic][0][0] - def get_header_time(self, msg): + def get_header_time(self, msg: genpy.Message): """ Retrieve header time if available - """ + Args: + msg (genpy.Message): The ROS message from which to extract the time. + + Returns: + rospy.Time: The timestamp of the topic's header if the topic + has a header. Otherwise, the current time is returned. + """ if hasattr(msg, "header"): return msg.header.stamp else: @@ -240,7 +280,8 @@ def get_header_time(self, msg): def get_time_index(self, topic, requested_seconds): """ - Return the index for the time index for a topic at 'n' seconds from the end of the dequeue + Returns the index for the time index for a topic at 'n' seconds from the end of the dequeue. + For example, to bag the last 10 seconds of data, the index for 10 seconds back from the most recent message can be obtained with this function. The number of requested seconds should be the number of seoncds desired from @@ -248,6 +289,15 @@ def get_time_index(self, topic, requested_seconds): If the desired time length of the bag is greater than the available messages it will output a message and return how ever many seconds of data are available at the moment. Seconds is of a number type (not a rospy.Time type) (ie. int, float) + + Args: + topic (str): The name of the topic for which to get the time index. + requested_seconds (int/float): The number of seconds from the end of the dequeue to search + for the topic. + + Returns: + int: The index for the time index of the topic at requested_seconds seconds from the + end of the dequeue. """ topic_duration = self.get_topic_duration(topic).to_sec() @@ -259,12 +309,17 @@ def get_time_index(self, topic, requested_seconds): index = int(self.get_topic_message_count(topic) * (1 - min(ratio, 1))) return index - def bagger_callback(self, msg, topic): + def bagger_callback(self, msg: genpy.Message, topic: str): """ - Streaming callback function, stops streaming during bagging process - also pops off msgs from dequeue if stream size is greater than specified stream_time + Adds incoming messages to the appropriate topic and removes older messages if necessary. + + Streaming callback function, stops streaming during bagging process and pops off msgs + from dequeue if stream size is greater than specified stream_time. Stream, callback + function does nothing if streaming is not active. - Stream, callback function does nothing if streaming is not active + Args: + msg (genpy.Message): The incoming message. + topic (str): The topic to which the incoming message will be added. """ if not self.streaming: @@ -280,11 +335,7 @@ def bagger_callback(self, msg, topic): # verify streaming is popping off and recording topics if self.iteration_count % 100 == 0: rospy.logdebug( - "{} has {} messages spanning {} seconds".format( - topic, - self.get_topic_message_count(topic), - round(time_diff.to_sec(), 2), - ), + f"{topic} has {self.get_topic_message_count(topic)} messages spanning {round(time_diff.to_sec(), 2)} seconds", ) while ( @@ -294,9 +345,16 @@ def bagger_callback(self, msg, topic): self.topic_messages[topic].popleft() time_diff = self.get_topic_duration(topic) - def get_topic_message_count(self, topic): + def get_topic_message_count(self, topic: str): """ Return number of messages available in a topic + + Args: + topic (str): The name of the topic for which to calculate the number + of messages. + + Returns: + int: The number of messages available in the specified topic. """ return len(self.topic_messages[topic]) @@ -304,8 +362,10 @@ def get_topic_message_count(self, topic): def get_total_message_count(self): """ Returns total number of messages across all topics - """ + Returns: + int: The total number of messages available in all topics. + """ total_message_count = 0 for topic in self.topic_messages: total_message_count = total_message_count + self.get_topic_message_count( @@ -315,14 +375,28 @@ def get_total_message_count(self): return total_message_count def _get_default_filename(self): + """ + Uses the current date and time to create a default bag name. + + Returns: + str: The default bag name constructed using format date-time. + """ return ( str(datetime.date.today()) + "-" + str(datetime.datetime.now().time())[0:8] ) - def get_bag_name(self, filename=""): + def get_bag_name(self, filename: str = ""): """ Create ros bag save directory If no bag name is provided, the current date/time is used as default. + + Args: + filename (str): The save directory for the ros bag. The default value + is an empty string, which will result in the default filename being + used. + + Returns: + str: A string representing the path of the ros bag file. """ # If directory param is not set, default to $HOME/bags/ default_dir = self.dir @@ -347,11 +421,19 @@ def get_bag_name(self, filename=""): bag_name = bag_name + ".bag" return os.path.join(bag_dir, bag_name) - def start_bagging(self, req): + def start_bagging(self, req: BagOnlineGoal): """ + Writes collected data to a bag file. + Dump all data in dictionary to bags, temporarily stops streaming during the bagging process, resumes streaming when over. - If bagging is already false because of an active call to this service + If bagging is already false because of an active call to this service. + + Args: + req (BagOnlineGoal): The bagging request information. + + Raises: + IOError: A problem occurs when opening or closing the bag file. """ result = BagOnlineResult() if self.streaming is False: diff --git a/mil_common/utils/mil_tools/nodes/tf_fudger.py b/mil_common/utils/mil_tools/nodes/tf_fudger.py index 0ae508c98..b526927f6 100755 --- a/mil_common/utils/mil_tools/nodes/tf_fudger.py +++ b/mil_common/utils/mil_tools/nodes/tf_fudger.py @@ -142,13 +142,9 @@ def reset(): q = tf.transformations.quaternion_from_euler(*rpy) if (p != p_last) or (rpy != rpy_last): - rpy_feedback = "xyz: {}, euler: {}".format( - [round(e, 5) for e in p], - [round(np.degrees(f), 5) for f in rpy], - ) - q_feedback = "xyz: {}, q: {}".format( - [round(x, 5) for e in p], - [round(f, 5) for f in q], + rpy_feedback = f"xyz: {[round(e, 5) for e in p]}, euler: {[round(np.degrees(f), 5) for f in rpy]}" + q_feedback = ( + f"xyz: {[round(x, 5) for e in p]}, q: {[round(f, 5) for f in q]}" ) print(q_feedback if q_mode else rpy_feedback) p_last = p diff --git a/mil_common/utils/mil_tools/nodes/tf_to_gazebo.py b/mil_common/utils/mil_tools/nodes/tf_to_gazebo.py index 9363d3c4f..a2c8f53d7 100755 --- a/mil_common/utils/mil_tools/nodes/tf_to_gazebo.py +++ b/mil_common/utils/mil_tools/nodes/tf_to_gazebo.py @@ -31,14 +31,7 @@ def main(): euler = tf.transformations.euler_from_quaternion(rot) print(f"(Roll={euler[0]}, Pitch={euler[1]}, Yaw={euler[2]})") print( - " {} {} {} {} {} {} ".format( - trans[0], - trans[1], - trans[2], - euler[0], - euler[1], - euler[2], - ), + f" {trans[0]} {trans[1]} {trans[2]} {euler[0]} {euler[1]} {euler[2]} ", ) except ( tf.LookupException, diff --git a/mil_common/utils/mil_tools/nodes/video_player.py b/mil_common/utils/mil_tools/nodes/video_player.py index 7b07ec3e7..9661bcd98 100755 --- a/mil_common/utils/mil_tools/nodes/video_player.py +++ b/mil_common/utils/mil_tools/nodes/video_player.py @@ -101,12 +101,7 @@ def __init__(self): self.roi_width_cb, ) rospy.loginfo( - "Playing {} at {}fps starting at frame {} ({} Total Frames)".format( - self.filename, - self.fps, - self.start_frame, - self.num_frames, - ), + f"Playing {self.filename} at {self.fps}fps starting at frame {self.start_frame} ({self.num_frames} Total Frames)", ) def run(self): diff --git a/mil_common/utils/mil_tools/scripts/mil-preflight/helper.py b/mil_common/utils/mil_tools/scripts/mil-preflight/helper.py new file mode 100644 index 000000000..bb5cf6ce9 --- /dev/null +++ b/mil_common/utils/mil_tools/scripts/mil-preflight/helper.py @@ -0,0 +1,222 @@ +################################################################################ +# File name: helper.py +# Author: Keith Khadar +# Description: This is used to store the helper functions for preflight +################################################################################ +# ----- Imports ----- # +# ----- Console -----# +# ----- Async -----# +import asyncio +import subprocess +import time + +# ----- Misc -----# +from contextlib import suppress + +# ----- Preflight -----# +import menus + +# ----- ROS -----# +import rospy +import rostopic +import tests +from axros import NodeHandle +from PyInquirer import prompt +from rich.console import Console +from rich.progress import Progress, track +from rich.table import Table + +# ----- Variables ----- # +report = [] + + +# ----- Functions ----- # +def clear_screen(): + # Clears the screen and prints the preflight header + subprocess.run("clear", shell=True) + Console().print(menus.title) + + +def init_report(): + # Function to initialize the report + report.clear() + + +def init_ros(): + # Checks if ROS is running and initializes it. + # Returns False if ROS is not running and TRUE if the ROS node was properly setup. + + # Check that ROS is running! + try: + rostopic._check_master() + except Exception: + Console().print("[bold] ROS not running! Please try again later[/]") + prompt(menus.press_anykey) + return False + + # Initialize the ROS node + with suppress(Exception): + rospy.init_node("preflight") + return True + + +async def axros_check_nodes(nodes): + # Asynchronously check all the nodes and then print/save the results + + # Setup AXROS + nh = NodeHandle.from_argv("Preflight_nh", "", anonymous=True) + + # Using async.io check all the nodes + answers = [] + async with nh: + # Create all the function calls to check_node + tasks = [check_node(node, answers, nh) for node in nodes] + + # Go through all the function calls and await them. + # Using track to display a loading bar. + for task in track( + asyncio.as_completed(tasks), + description="Checking Nodes...", + total=len(tasks), + ): + await task + + # Clear the screen, print and save the response to the report + print_results(answers, "Node Liveliness") + + +async def check_node(node, results, nh): + # Using AXROS lookup a node and save the result in the results list + + try: + results.append((node, bool(await nh.lookup_node(node)))) + except Exception: + results.append((node, False)) + + +async def axros_check_topics(topics): + # Asynchronously check all the topics and then print/save the results + + # Setup AXROS + nh = NodeHandle.from_argv("Preflight_nh", "", anonymous=True) + + # Using async.io check all the topics + answers = [] + async with nh: + # Create all the function calls to check topic + tasks = [check_topic(topic, answers, nh) for topic in topics] + + # Go through all the function calls and await them. + # Using track to display a loading bar. + for task in track( + asyncio.as_completed(tasks), + description="Checking Topics...", + total=len(tasks), + ): + await task + + # Clear the screen, print and save the response to the report + print_results(answers, "Topic Liveliness") + + +async def check_topic(topic, results, nh): + # Using AXROS subscribe to a topic and wait for a message + + # Get the topic class + topicType, topicStr, _ = rostopic.get_topic_class(topic) + + # Create an AXROS subscriber + sub = nh.subscribe(topicStr, topicType) + + async with sub: + try: + await asyncio.wait_for(sub.get_next_message(), tests.topic_timeout) + results.append((topic, True)) + except Exception: + results.append((topic, False)) + + +def check_actuators(actuators): + # Check all the actuators using check_actuator + + answers = [] + for actuator in actuators: + check_actuator(actuator, answers) + + # Clear the screen, print and save the response to the report + print_results(answers, "Actuator Tests") + + +def check_actuator(actuator, results): + # Checks the actuator by publishing to a topic for a specified time + + try: + # Confirm that it is safe to run this actuator + Console().print(menus.safety_check, actuator[0]) + menu_ans = prompt(menus.continue_question) + if next(iter(menu_ans.values())) is False: + # Go back to main menu + return + + # Create a publisher + topicType, topicStr, _ = rostopic.get_topic_class(actuator[1][0]) + pub = rospy.Publisher(topicStr, topicType, queue_size=10) + + # Publish to the topic for the specified timeout + with Progress() as progress: + t_start = time.time() + t_end = t_start + tests.actuator_timeout + t_prev = time.time() + task = progress.add_task("Running", total=(t_end - t_start)) + while time.time() <= t_end: + pub.publish(actuator[1][1]) + progress.update(task, advance=(time.time() - t_prev)) + t_prev = time.time() + progress.update(task, advance=t_end) + + # Ask if the actuator worked + Console().print(menus.actuator_check) + results.append((actuator[0], next(iter(prompt(menus.yes_no).values())))) + except Exception: + Console().print(menus.actuator_failed) + results.append((actuator[0], False)) + + +def generate_report(): + # Attempts to create and display the report + + # Check that there is a report + if len(report) == 0: + Console().print( + "[bold]No report![/].\nPlease generate a report by running a full test.", + ) + prompt(menus.press_anykey) + return + # Generate the report + for result in report: + Console().print(result) + prompt(menus.press_anykey_menu_return) + + +def print_results(systems, name): + # This saves the result to the specified system and prints it + clear_screen() + result = create_result(systems, name) + Console().print(result) + + +def create_result(systems, name): + # This save the result into a RICH table + + # Generates a table to hold information about each system + result = Table(title=f"[bold]{name}[/]") + result.add_column("System Name", justify="center", style="cyan", no_wrap=True) + result.add_column("Status", justify="center", style="magenta", no_wrap=True) + + # Populates the table + for system, status in systems: + status_text = "[green]✔ Working[/]" if status else "[red]❌ Not Working[/]" + result.add_row(system, status_text) + report.append(result) + + return result diff --git a/mil_common/utils/mil_tools/scripts/mil-preflight/main.py b/mil_common/utils/mil_tools/scripts/mil-preflight/main.py new file mode 100644 index 000000000..b856c5028 --- /dev/null +++ b/mil_common/utils/mil_tools/scripts/mil-preflight/main.py @@ -0,0 +1,237 @@ +################################################################################ +# File name: main.py +# Author: Keith Khadar +# Description: This file is the entry point to preflight. +################################################################################ +# ----- Imports ----- # +# ----- Console -----# +# ----- Async -----# +import asyncio +import subprocess + +# ----- Misc -----# +from pathlib import Path + +import helper + +# ----- Preflight -----# +import menus +import tests +from PyInquirer import prompt +from rich.console import Console +from rich.markdown import Markdown + + +# ----- Main Routine ----- # +async def main(): + + # Clear Screen and Display Start menu + helper.clear_screen() + + # Print Info about Preflight + Console().print(menus.info_page) + + # Print start select menu + option = prompt(menus.start_menu) + try: + mode = next(iter(option.values())) + # Select the mode and run it + if mode == "Run Preflight Full Test": + await fullTest() + if mode == "View Report": + viewReport() + if mode == "Run Specific Test": + await specificTest() + if mode == "View Documentation": + viewDocumentation() + if mode == "Exit": + subprocess.run("clear", shell=True) + return + except StopIteration: + # Return if the user presses Ctrl-c + return + except Exception: + pass + + # Return to this screen after running the selected mode + await main() + + +# ----- Subroutines ----- # + + +# ----- Modes -----# +async def fullTest(): + + helper.init_report() + + ### Complete the setup tests ### + + # Clear the screen and display the setup checklist + helper.clear_screen() + Console().print(menus.setup_desc) + respond = prompt(menus.setupChecklist) + + # Filter the response and store checklist to the report + answers = [] + for i in range(len(tests.setup)): + if tests.setup[i] in next(iter(respond.values())): + answers.append((tests.setup[i], True)) + else: + answers.append((tests.setup[i], False)) + helper.create_result(answers, "Setup Checklist") + + # Check if the list is incomplete. If so prompt user for confirmation to continue + if len(next(iter(respond.values()))) != len(tests.setup): + menu_ans = prompt(menus.incomplete_continue) + if next(iter(menu_ans.values())) is False: + return + + ### Complete Software Tests ### + if not helper.init_ros(): + return + + # Clear the screen + helper.clear_screen() + + # Print Node Screen description + Console().print(menus.node_desc) + + # Check Nodes + await helper.axros_check_nodes(tests.nodes) + + # Prompt the user to continue to next test + menu_ans = prompt(menus.continue_question) + if next(iter(menu_ans.values())) is False: + # Go back to main menu + return + + # Print Topic screen description + helper.clear_screen() + Console().print(menus.topic_desc) + + # Check Topics + + await helper.axros_check_topics(tests.topics) + + # Prompt the user to continue to next test + menu_ans = prompt(menus.continue_question) + if next(iter(menu_ans.values())) is False: + # Go back to main menu + return + + ### Actuators Test ### + # Print Actuators Screen description + helper.clear_screen() + Console().print(menus.node_desc) + + helper.check_actuators(tests.actuatorsList) + + prompt(menus.press_anykey_menu_return) + return + + +def viewReport(): + # Clear the screen + helper.clear_screen() + + # Generate the report + helper.generate_report() + return + + +def viewDocumentation(): + # Clear the screen + helper.clear_screen() + + # Find path to README from current directory + mod_path = Path(__file__).parent + rel_path = "preflight.md" + src_path = (mod_path / rel_path).resolve() + # Print the documentation + with open(src_path, "r+") as help_file: + Console().print(Markdown(help_file.read())) + + prompt(menus.press_anykey_menu_return) + return + + +async def specificTest(): + # Init the report + helper.init_report() + + # Clear the screen and display the node checklist + helper.clear_screen() + + helper.init_ros() + + # Clear the screen and display the node checklist + helper.clear_screen() + Console().print(menus.specific_desc) + respond = prompt(menus.nodeChecklist) + + # Filter the response and store checklist to the report + nodes = [] + for i in range(len(tests.nodes)): + if tests.nodes[i] in next(iter(respond.values())): + nodes.append(tests.nodes[i]) + + # Print Node Screen description + Console().print(menus.node_desc) + + # Check Nodes + await helper.axros_check_nodes(nodes=nodes) + + # Prompt the user to continue to next test + menu_ans = prompt(menus.continue_question) + if next(iter(menu_ans.values())) is False: + # Go back to main menu + return + + # Clear the screen and display the topic checklist + helper.clear_screen() + Console().print(menus.specific_desc) + respond = prompt(menus.topicChecklist) + + # Filter the response and store checklist to the report + topics = [] + for i in range(len(tests.topics)): + if tests.topics[i] in next(iter(respond.values())): + topics.append(tests.topics[i]) + + # Print Topic screen description + helper.clear_screen() + Console().print(menus.topic_desc) + + # Check Topics + await helper.axros_check_topics(topics=topics) + + # Prompt the user to continue to next test + menu_ans = prompt(menus.continue_question) + if next(iter(menu_ans.values())) is False: + # Go back to main menu + return + + # Clear the screen and display the actuator checklist + helper.clear_screen() + Console().print(menus.specific_desc) + respond = prompt(menus.actuatorChecklist) + + # Filter the response and store checklist to the report + actuators = [] + for i in range(len(tests.actuatorsList)): + if tests.actuatorsList[i][0] in next(iter(respond.values())): + actuators.append(tests.actuatorsList[i]) + + # Print Actuators Screen description + helper.clear_screen() + Console().print(menus.node_desc) + + helper.check_actuators(actuators=actuators) + + prompt(menus.press_anykey_menu_return) + return + + +if __name__ == "__main__": + asyncio.run(main()) diff --git a/mil_common/utils/mil_tools/scripts/mil-preflight/menus.py b/mil_common/utils/mil_tools/scripts/mil-preflight/menus.py new file mode 100644 index 000000000..53a3cd738 --- /dev/null +++ b/mil_common/utils/mil_tools/scripts/mil-preflight/menus.py @@ -0,0 +1,147 @@ +################################################################################ +# File name: menus.py +# Author: Keith Khadar +# Description: This file is used to store all the menus used by preflight. Version 1.0 +################################################################################ +# ----- Imports -----# +import tests + +# ----- Home Page -----# +title = """ +[bold green]Preflight Program - Autonomous Robot Verification[/bold green] +""" +info_page = """ +Welcome to the Preflight Program, a tool inspired by the preflight checklists used by pilots before flying a plane. This program is designed to verify the functionality of all software and hardware systems on your autonomous robot. It ensures that everything is in working order, allowing you to safely deploy your robot with confidence.\n +""" + + +start_menu = [ + { + "type": "list", + "name": "mode selection", + "message": "Menu", + "choices": [ + "Run Preflight Full Test", + "View Report", + "Run Specific Test", + "View Documentation", + "Exit", + ], + }, +] + +# ----- Loading Screen -----# +continue_question = [ + { + "type": "confirm", + "name": "continue", + "message": "Continue?", + }, +] +press_anykey = [ + { + "type": "confirm", + "name": "continue", + "message": "Press any key to continue.", + "confirm": False, + }, +] +press_anykey_menu_return = [ + { + "type": "confirm", + "name": "continue", + "message": "Press any key to return to the menu.", + "confirm": False, + }, +] + +incomplete_continue = [ + { + "type": "confirm", + "name": "incomplete", + "message": "This checklist is incomplete. Do you wish to continue?", + }, +] +yes_no = [ + { + "type": "confirm", + "name": "yes_no", + "message": "Yes?", + }, +] + + +# ----- Software Screen -----# +node_desc = """ +Welcome to the [bold]ROS Node Liveliness Test screen[/]. This screen allows you to verify the liveliness of ROS nodes within the system. ROS nodes are essential components responsible for communication and control within the robot\'s architecture. Use this screen to ensure that all necessary nodes are running and responsive for proper system functionality. +""" +topic_desc = """ +Welcome to the Topic Monitoring screen. This screen allows you to monitor the topics in the ROS system. In a ROS (Robot Operating System) environment, topics serve as channels through which nodes communicate by publishing and subscribing to messages. Monitoring topics enables you to verify that sensors are actively publishing their data. +""" + +nodeChecklist = [ + { + "type": "checkbox", + "message": "Node Checklist:", + "name": "NodeChecklist: \nPlease check that all of the following are in working order.", + "choices": [{"name": item} for item in tests.nodes], + }, +] + +topicChecklist = [ + { + "type": "checkbox", + "message": "Topic Checklist:", + "name": "TopicChecklist: \nPlease check that all of the following are in working order.", + "choices": [{"name": item} for item in tests.topics], + }, +] + +# ----- Setup Screen -----# +setup_desc = """ +Welcome to the [bold]Setup Checklist[/] page of our preflight app! This checklist is designed to ensure that all critical hardware components of your robot are thoroughly inspected before each operation. By completing this checklist, you contribute to the safety and reliability of the robot during its mission. Please carefully examine each item listed below to verify its condition and functionality. + +Go through each item and check it off. Use the arrow keys to select and item and press space to check it off. Once all items have been checked press enter to continue. You can always review what items you checked off later in the report section of the main menu. + +""" +setupChecklist = [ + { + "type": "checkbox", + "message": "Setup Checklist:", + "name": "HardwareTests: \nPlease check that all of the following are in working order.", + "choices": [{"name": item} for item in tests.setup], + }, +] + +# ----- Actuators Screen -----# +actuator_desc = """ +Welcome to the [bold]Actuator Test Screen[/]. This screen allows you to test the functionality of various actuators on the robot. Actuators play a crucial role in the movement and operation of the robot. Use this screen with caution and follow the instructions carefully to avoid any accidents or damage to the robot. +\n[bold red]Caution:[/bold red] Actuators can be dangerous if mishandled. Please be careful when testing them, as they have the potential to cause harm or injury. For example, thrusters, when spinning, could chop off someone's finger. Always follow safety protocol and guidelines. +""" +safety_check = """ +[bold][yellow]Ensure that all fingers are clear of the area![/yellow][/bold] Is it safe to operate the following actuator? +""" + +actuator_check = """ +Did the actuator work? +""" + +actuator_failed = """ +Actuator failed! +""" + +actuatorChecklist = [ + { + "type": "checkbox", + "message": "Actuator Checklist:", + "name": "ActuatorTests: \nPlease check that all of the following are in working order.", + "choices": [{"name": item[0]} for item in tests.actuatorsList], + }, +] + +# ----- Specific Test Screen -----# +specific_desc = """ +Welcome to the [bold]Specific Test[/] page of our preflight app! Here you can specify which tests you want to run. This can be useful for debugging parts of the robot. +You can use the arrow keys to select which tests you want to run (use the spacebar to select). Once you are ready you can press enter to run those tests! + +""" diff --git a/mil_common/utils/mil_tools/scripts/mil-preflight/preflight.md b/mil_common/utils/mil_tools/scripts/mil-preflight/preflight.md new file mode 100644 index 000000000..942d85cb4 --- /dev/null +++ b/mil_common/utils/mil_tools/scripts/mil-preflight/preflight.md @@ -0,0 +1,24 @@ +# Preflight- Autonomous Robot Verification +## How to Use +Simply type "preflight" anywhere in the MIL directory. Make sure that a robot is connected and running or gazebo is running. To edit the hardware tests list and the automated software tests, edit the tests.py file in mil/mil_common/utils/mil_tools/scripts/mil-preflight/tests.py +## Description +Preflight is an automated testing tool that should be run after turning on and connecting to the robot to run a prelaunch hardware checklist and automated software checklist. + +### There are three types of automated software tests +#### Actuators +This test will prompt the user to enable physical moving actuators on the robot. Make sure that the area is cleared and the robot won't damage itself or others nearby. The user will have to watch and validate that they move as expected personally. + +To add an actuator test, add a topic with a list of commands and import the module it comes from. See the thrusters example for reference in tests.py +#### Nodes +A ROS Node is a live process currently running and performing a task. They communicate with each other through topics, services, and messages, etc. This test will ensure all listed ROS Nodes are running and alive as expected. + +To add a Node test, add a node to the list in tests.py +#### Topics +ROS Topics act as a channel for ROS Nodes to communicate by publishing and subscribing to messages. These tests check to verify that data is being published to these topics, ensuring that sensors under the listed topics are properly reading publishing data. + +To add a Topic test, add a topic to the list in tests.py + +### Setup Tests +There are also setup tests. These are used to verify certain features on the robot that cannot be automated. For example ensuring that the O-rings are greased. + +To add a Setup test, add a what need to be tested to the list in tests.py diff --git a/mil_common/utils/mil_tools/scripts/mil-preflight/tests.py b/mil_common/utils/mil_tools/scripts/mil-preflight/tests.py new file mode 100644 index 000000000..9503cf5cd --- /dev/null +++ b/mil_common/utils/mil_tools/scripts/mil-preflight/tests.py @@ -0,0 +1,94 @@ +################################################################################ +# File name: tests.py +# Author: Keith Khadar +# Description: This file is used to store tests used by Preflight. If you want +# add tests you would add them here. +################################################################################ + +# ----- Actuator Tests ----- # +# Add tests here for actuators. These will be turned on and the user +# will confirm their operation. Also include any custom message + +# imports here. + +# Thruster Messages +from subjugator_msgs.msg import ThrusterCmd + +# ----- Timeouts ----- # +node_timeout = 5 # seconds +topic_timeout = 5 # seconds +actuator_timeout = 1.5 # seconds + +actuatorsList = [ + ### ( + ### "Name of Test", + ### ("/topic", message), + ### ) + ( + "FLH Thruster Test", + ("/thrusters/thrust", [ThrusterCmd(name="FLH", thrust=10.0)]), + ), + ( + "FRH Thruster Test", + ("/thrusters/thrust", [ThrusterCmd(name="FRH", thrust=10.0)]), + ), + ( + "BLH Thruster Test", + ("/thrusters/thrust", [ThrusterCmd(name="BLH", thrust=10.0)]), + ), + ( + "BRH Thruster Test", + ("/thrusters/thrust", [ThrusterCmd(name="BRH", thrust=10.0)]), + ), + ( + "FLV Thruster Test", + ("/thrusters/thrust", [ThrusterCmd(name="FLV", thrust=10.0)]), + ), + ( + "FRV Thruster Test", + ("/thrusters/thrust", [ThrusterCmd(name="FRV", thrust=10.0)]), + ), + ( + "BLV Thruster Test", + ("/thrusters/thrust", [ThrusterCmd(name="BLV", thrust=10.0)]), + ), + ( + "BRV Thruster Test", + ("/thrusters/thrust", [ThrusterCmd(name="BRV", thrust=10.0)]), + ), +] + + +# ----- Setup Tests ----- # +# Add tests here for things that need to be physically inspected or check before the sub is running +setup = [ + "Grease O-rings with Molykote 55 every time a pressure vessel is closed.", + "Deploy sub. (Check for bubbles coming out of every pressure vessel, make sure buoyancy is correct)", +] + + +# ----- Software Tests ----- # +# Add tests here for software systems like sensors whose operations +# need to be verified. + +# ----- Nodes -----# +nodes = [ + # Navbox processing + "/odom_estimator", + "/transform_odometry", + "/c3_trajectory_generator", + "/adaptive_controller", + "/thruster_mapper", + "/mission_runner", +] + +# ----- Topics -----# +topics = [ + "/camera/front/right/image_raw", + "/camera/down/image_raw", + "/camera/front/left/image_raw", + "/dvl", + "/depth", + "/imu/data_raw", + "/imu/mag", +] diff --git a/requirements.txt b/requirements.txt index 6ff0b0ecc..93cf473b8 100644 --- a/requirements.txt +++ b/requirements.txt @@ -15,7 +15,7 @@ scipy==1.10.0 scikit-learn==1.1.1 # Computer Vision -Pillow==10.2.0 +Pillow==10.3.0 # File Handling PyYAML==6.0 @@ -31,11 +31,16 @@ breathe==4.34.0 myst-parser==0.18.0 sphinx-copybutton==0.5.0 +# Terminal +PyInquirer==1.0.3 +rich==13.7.1 +pygments==2.17.2 + # External Devices pyserial==3.5 # Linting -black==22.1.0 +black==24.3.0 modernize==0.8.0 # This can be removed after Noetic Migration is complete. # Testing diff --git a/scripts/SubChecklist.txt b/scripts/SubChecklist.txt new file mode 100644 index 000000000..2d9664d4a --- /dev/null +++ b/scripts/SubChecklist.txt @@ -0,0 +1,2 @@ +dvl +odom diff --git a/scripts/install.sh b/scripts/install.sh index 4e60999e6..b21bd4b24 100755 --- a/scripts/install.sh +++ b/scripts/install.sh @@ -248,7 +248,8 @@ mil_user_install_dependencies() { fd-find \ ripgrep \ fzf \ - aptitude + aptitude \ + lm-sensors } # Add line to user's bashrc which source the repo's setup files diff --git a/scripts/setup.bash b/scripts/setup.bash index f164bb84d..f2f1f05a7 100755 --- a/scripts/setup.bash +++ b/scripts/setup.bash @@ -45,8 +45,9 @@ source "$MIL_WS/src/mil/NaviGator/scripts/bash_aliases.sh" # Repo aliases alias mil='cd $MIL_REPO' -alias cm='catkin_make -C $MIL_WS' +alias cm='catkin_make -DCATKIN_WHITELIST_PACKAGES="" -C $MIL_WS' alias vrx='cd $MIL_REPO/NaviGator/simulation/VRX/vrx' +alias cputemp='watch sensors' # General ROS aliases ros_env() { @@ -89,6 +90,9 @@ alias fd="fdfind" # Gazebo aliases alias gazebogui="rosrun gazebo_ros gzclient __name:=gzclient" +# Preflight aliases +alias preflight='python3 $MIL_REPO/mil_common/utils/mil_tools/scripts/mil-preflight/main.py' + # Process killing aliases alias killgazebo="killall -9 gzserver && killall -9 gzclient" alias killros='$MIL_REPO/scripts/kill_ros.sh'