From 2c861cb6f50379e8b09c3042f97128557c25f584 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Sat, 8 Jun 2024 02:31:59 +0000 Subject: [PATCH 1/4] Update pre-commit hooks to latest versions (#1200) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit * Update pre-commit hooks to latest versions updates: - [github.com/pre-commit/mirrors-clang-format: v18.1.4 → v18.1.5](https://github.com/pre-commit/mirrors-clang-format/compare/v18.1.4...v18.1.5) - [github.com/astral-sh/ruff-pre-commit: v0.4.3 → v0.4.7](https://github.com/astral-sh/ruff-pre-commit/compare/v0.4.3...v0.4.7) - [github.com/codespell-project/codespell: v2.2.6 → v2.3.0](https://github.com/codespell-project/codespell/compare/v2.2.6...v2.3.0) * [pre-commit.ci] auto fixes from pre-commit.com hooks for more information, see https://pre-commit.ci * pre-commit changes --------- Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> Co-authored-by: Cameron Brown --- .pre-commit-config.yaml | 8 ++-- .../navigator_missions/detect_deliver_find.py | 2 +- .../vrx_missions/scan_the_code.py | 2 +- .../nodes/torpedo_board.cpp | 48 ++++++++++++------- .../subjugator_vision_lib/object_finder.cpp | 27 +++++++---- .../subjugator_vision_tools/estimation.py | 4 +- .../src/Classification.cpp | 2 +- .../src/subjugator_buoyancy.cc | 3 +- .../subjugator_gazebo/src/subjugator_test.cc | 13 ++--- .../mil_usb_to_can/sub8/board.py | 2 +- mil_common/gnc/odom_estimator/data/readme.txt | 2 +- .../mil_vision/src/mil_vision_lib/cv_utils.cc | 22 ++++++--- mil_common/utils/mil_tools/nodes/tf_fudger.py | 4 +- 13 files changed, 88 insertions(+), 51 deletions(-) diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index aa1314177..d79b3efee 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -19,7 +19,7 @@ repos: hooks: - id: black - repo: https://github.com/pre-commit/mirrors-clang-format - rev: v18.1.4 + rev: v18.1.5 hooks: - id: clang-format - repo: https://github.com/PyCQA/autoflake @@ -40,16 +40,16 @@ repos: exclude: ^docker|deprecated|NaviGator/simulation/VRX - repo: https://github.com/astral-sh/ruff-pre-commit # Ruff version. - rev: 'v0.4.3' + rev: 'v0.4.7' hooks: - id: ruff args: [--fix, --exit-non-zero-on-fix] - repo: https://github.com/codespell-project/codespell - rev: v2.2.6 + rev: v2.3.0 hooks: - id: codespell args: - - --ignore-words-list=fpr,ser,wan,te,wan,ba,ned,ans,hist,nd,wronly + - --ignore-words-list=fpr,ser,wan,te,wan,ba,ned,ans,hist,nd,wronly,Voight,assertIn - --skip="./.*,*.csv,*.json" - --quiet-level=2 exclude_types: [csv, json] diff --git a/NaviGator/mission_control/navigator_missions/navigator_missions/detect_deliver_find.py b/NaviGator/mission_control/navigator_missions/navigator_missions/detect_deliver_find.py index c6d0bccd7..1b768d136 100644 --- a/NaviGator/mission_control/navigator_missions/navigator_missions/detect_deliver_find.py +++ b/NaviGator/mission_control/navigator_missions/navigator_missions/detect_deliver_find.py @@ -87,7 +87,7 @@ async def run(self, args): override_scale = args.overridescale pre_circle = args.circle scan_dist = args.scandist - look_in = args.lookin + look_in = args.lookin # codespell:ignore end_dist = args.enddist # Turn on ML classifier for docks diff --git a/NaviGator/mission_control/navigator_missions/vrx_missions/scan_the_code.py b/NaviGator/mission_control/navigator_missions/vrx_missions/scan_the_code.py index 72fdb5088..19910ec11 100644 --- a/NaviGator/mission_control/navigator_missions/vrx_missions/scan_the_code.py +++ b/NaviGator/mission_control/navigator_missions/vrx_missions/scan_the_code.py @@ -182,7 +182,7 @@ async def report_sequence(self, sequence): try: await self.sequence_report(color_sequence) - except Exception as e: # catch error in case vrx scroing isn't running + except Exception as e: # catch error in case vrx scoring isn't running print(e) async def find_stc2(self): diff --git a/SubjuGator/perception/subjugator_perception/nodes/torpedo_board.cpp b/SubjuGator/perception/subjugator_perception/nodes/torpedo_board.cpp index 47f71bfb6..51f57e387 100644 --- a/SubjuGator/perception/subjugator_perception/nodes/torpedo_board.cpp +++ b/SubjuGator/perception/subjugator_perception/nodes/torpedo_board.cpp @@ -33,19 +33,24 @@ try : image_transport(nh), rviz("/torpedo_board/visualization/detection") // Set image processing scale image_proc_scale = param("/torpedo_vision/img_proc_scale", image_proc_scale_default); - log_msg << setw(1 * tab_sz) << "" << "Image Processing Scale: \x1b[37m" << image_proc_scale << "\x1b[0m\n"; + log_msg << setw(1 * tab_sz) << "" + << "Image Processing Scale: \x1b[37m" << image_proc_scale << "\x1b[0m\n"; // Set diffusion duration in pseudotime diffusion_time = param("/torpedo_vision/diffusion_time", diffusion_time_default); - log_msg << setw(1 * tab_sz) << "" << "Anisotropic Diffusion Duration: \x1b[37m" << diffusion_time << "\x1b[0m\n"; + log_msg << setw(1 * tab_sz) << "" + << "Anisotropic Diffusion Duration: \x1b[37m" << diffusion_time << "\x1b[0m\n"; // Set feature extraction parameters max_features = param("/torpedo_vision/max_features", max_features_default); - log_msg << setw(1 * tab_sz) << "" << "Maximum features: \x1b[37m" << max_features << "\x1b[0m\n"; + log_msg << setw(1 * tab_sz) << "" + << "Maximum features: \x1b[37m" << max_features << "\x1b[0m\n"; feature_block_size = param("/torpedo_vision/feature_block_size", feature_block_size_default); - log_msg << setw(1 * tab_sz) << "" << "Feature Block Size: \x1b[37m" << feature_block_size << "\x1b[0m\n"; + log_msg << setw(1 * tab_sz) << "" + << "Feature Block Size: \x1b[37m" << feature_block_size << "\x1b[0m\n"; feature_min_distance = param("/torpedo_vision/feature_min_distance", feature_min_distance_default); - log_msg << setw(1 * tab_sz) << "" << "Feature Minimum Distance: \x1b[37m" << feature_min_distance << "\x1b[0m\n"; + log_msg << setw(1 * tab_sz) << "" + << "Feature Minimum Distance: \x1b[37m" << feature_min_distance << "\x1b[0m\n"; // Configure debug image generation generate_dbg_img = param("/torpedo_vision/generate_dbg_imgs", generate_dbg_img_default); @@ -57,21 +62,28 @@ try : image_transport(nh), rviz("/torpedo_board/visualization/detection") image_transport.subscribeCamera(left, 10, &SubjuGatorTorpedoBoardDetector::left_image_callback, this); right_image_sub = image_transport.subscribeCamera(right, 10, &SubjuGatorTorpedoBoardDetector::right_image_callback, this); - log_msg << setw(1 * tab_sz) << "" << "Camera Subscriptions:\x1b[37m\n" - << setw(2 * tab_sz) << "" << "left = " << left << endl - << setw(2 * tab_sz) << "" << "right = " << right << "\x1b[0m\n"; + log_msg << setw(1 * tab_sz) << "" + << "Camera Subscriptions:\x1b[37m\n" + << setw(2 * tab_sz) << "" + << "left = " << left << endl + << setw(2 * tab_sz) << "" + << "right = " << right << "\x1b[0m\n"; // Register Pose Estimation Service Client string pose_est_srv = param("/torpedo_vision/pose_est_srv", pose_est_srv_default); pose_client = nh.serviceClient(pose_est_srv); - log_msg << setw(1 * tab_sz) << "" << "Registered as client of the service:\n" - << setw(2 * tab_sz) << "" << "\x1b[37m" << pose_est_srv << "\x1b[0m\n"; + log_msg << setw(1 * tab_sz) << "" + << "Registered as client of the service:\n" + << setw(2 * tab_sz) << "" + << "\x1b[37m" << pose_est_srv << "\x1b[0m\n"; // Advertise debug image topic string dbg_topic = param("/torpedo_vision/dbg_imgs", dbg_topic_default); debug_image_pub = image_transport.advertise(dbg_topic, 1, true); - log_msg << setw(1 * tab_sz) << "" << "Advertised debug image topic:\n" - << setw(2 * tab_sz) << "" << "\x1b[37m" << dbg_topic << "\x1b[0m\n"; + log_msg << setw(1 * tab_sz) << "" + << "Advertised debug image topic:\n" + << setw(2 * tab_sz) << "" + << "\x1b[37m" << dbg_topic << "\x1b[0m\n"; // Setup debug image quadrants int frame_height = param("/torpedo_vision/frame_height", frame_height_default); @@ -92,14 +104,17 @@ try : image_transport(nh), rviz("/torpedo_board/visualization/detection") string activation = param("/torpedo_vision/activation", activation_default); detection_switch = nh.advertiseService(activation, &SubjuGatorTorpedoBoardDetector::detection_activation_switch, this); - log_msg << setw(1 * tab_sz) << "" << "Advertised torpedo board detection switch:\n" - << setw(2 * tab_sz) << "" << "\x1b[37m" << activation << "\x1b[0m\n"; + log_msg << setw(1 * tab_sz) << "" + << "Advertised torpedo board detection switch:\n" + << setw(2 * tab_sz) << "" + << "\x1b[37m" << activation << "\x1b[0m\n"; // Start main detector loop run_id = 0; boost::thread main_loop_thread(boost::bind(&SubjuGatorTorpedoBoardDetector::run, this)); main_loop_thread.detach(); - log_msg << setw(1 * tab_sz) << "" << "Running main detector loop in a background thread\n"; + log_msg << setw(1 * tab_sz) << "" + << "Running main detector loop in a background thread\n"; log_msg << "SubjuGatorTorpedoBoardDetector Initialized\n"; ROS_INFO(log_msg.str().c_str()); @@ -222,7 +237,8 @@ void SubjuGatorTorpedoBoardDetector::determine_torpedo_board_position() right_stamp = right_most_recent.image_msg_ptr->header.stamp.toSec(); double sync_error = fabs(left_stamp - right_stamp); stringstream sync_msg; - sync_msg << "Left and right images were not sufficiently synchronized" << "\nsync error: " << sync_error << "s"; + sync_msg << "Left and right images were not sufficiently synchronized" + << "\nsync error: " << sync_error << "s"; if (sync_error > sync_thresh) { ROS_WARN(sync_msg.str().c_str()); diff --git a/SubjuGator/perception/subjugator_perception/src/subjugator_vision_lib/object_finder.cpp b/SubjuGator/perception/subjugator_perception/src/subjugator_vision_lib/object_finder.cpp index 5043656db..93db69df8 100644 --- a/SubjuGator/perception/subjugator_perception/src/subjugator_vision_lib/object_finder.cpp +++ b/SubjuGator/perception/subjugator_perception/src/subjugator_vision_lib/object_finder.cpp @@ -21,7 +21,8 @@ try : image_transport(nh), rviz("/torpedo_board/visualization/detection") // Set image processing scale image_proc_scale = ros::param::param("img_proc_scale", image_proc_scale_default); - log_msg << std::setw(1 * tab_sz) << "" << "Image Processing Scale: \x1b[37m" << image_proc_scale << "\x1b[0m\n"; + log_msg << std::setw(1 * tab_sz) << "" + << "Image Processing Scale: \x1b[37m" << image_proc_scale << "\x1b[0m\n"; // Configure debug image generation generate_dbg_img = ros::param::param("generate_dbg_imgs", generate_dbg_img_default); @@ -31,15 +32,20 @@ try : image_transport(nh), rviz("/torpedo_board/visualization/detection") std::string right = ros::param::param("input_right", img_topic_right_default); left_image_sub = image_transport.subscribeCamera(left, 1000, &SubjuGatorObjectFinder::left_image_callback, this); right_image_sub = image_transport.subscribeCamera(right, 1000, &SubjuGatorObjectFinder::right_image_callback, this); - log_msg << std::setw(1 * tab_sz) << "" << "Camera Subscriptions:\x1b[37m\n" - << std::setw(2 * tab_sz) << "" << "left = " << left << std::endl - << std::setw(2 * tab_sz) << "" << "right = " << right << "\x1b[0m\n"; + log_msg << std::setw(1 * tab_sz) << "" + << "Camera Subscriptions:\x1b[37m\n" + << std::setw(2 * tab_sz) << "" + << "left = " << left << std::endl + << std::setw(2 * tab_sz) << "" + << "right = " << right << "\x1b[0m\n"; // Advertise debug image topic std::string dbg_topic = ros::param::param("dbg_imgs", dbg_topic_default); debug_image_pub = image_transport.advertise(dbg_topic, 1, true); - log_msg << std::setw(1 * tab_sz) << "" << "Advertised debug image topic:\n" - << std::setw(2 * tab_sz) << "" << "\x1b[37m" << dbg_topic << "\x1b[0m\n"; + log_msg << std::setw(1 * tab_sz) << "" + << "Advertised debug image topic:\n" + << std::setw(2 * tab_sz) << "" + << "\x1b[37m" << dbg_topic << "\x1b[0m\n"; // Setup debug image quadrants cv::Size input_frame_size(644, 482); // This needs to be changed if we ever change @@ -57,14 +63,17 @@ try : image_transport(nh), rviz("/torpedo_board/visualization/detection") active = false; std::string activation = ros::param::param("activation", activation_default); detection_switch = nh.advertiseService(activation, &SubjuGatorObjectFinder::detection_activation_switch, this); - log_msg << std::setw(1 * tab_sz) << "" << "Advertised torpedo board detection switch:\n" - << std::setw(2 * tab_sz) << "" << "\x1b[37m" << activation << "\x1b[0m\n"; + log_msg << std::setw(1 * tab_sz) << "" + << "Advertised torpedo board detection switch:\n" + << std::setw(2 * tab_sz) << "" + << "\x1b[37m" << activation << "\x1b[0m\n"; // Start main detector loop run_id = 0; boost::thread main_loop_thread(boost::bind(&SubjuGatorObjectFinder::run, this)); main_loop_thread.detach(); - log_msg << std::setw(1 * tab_sz) << "" << "Running main detector loop in a background thread\n"; + log_msg << std::setw(1 * tab_sz) << "" + << "Running main detector loop in a background thread\n"; log_msg << "SubjuGatorObjectFinder Initialized\n"; ROS_INFO(log_msg.str().c_str()); diff --git a/SubjuGator/perception/subjugator_perception/subjugator_vision_tools/estimation.py b/SubjuGator/perception/subjugator_perception/subjugator_vision_tools/estimation.py index fbd9530be..69c38ec94 100644 --- a/SubjuGator/perception/subjugator_perception/subjugator_vision_tools/estimation.py +++ b/SubjuGator/perception/subjugator_perception/subjugator_vision_tools/estimation.py @@ -217,8 +217,8 @@ def observe(self, observation): expected_obs = expected_obs_h[:2, :] / expected_obs_h[2, :] # Check how many particles are in the field of view, if we have a problem, don't try - infront = np.sum(self.in_fov(particles)) - if infront < 5: + in_front = np.sum(self.in_fov(particles)) + if in_front < 5: # TODO: Do a partial ray-reset return diff --git a/SubjuGator/perception/subjugator_pointcloud/src/Classification.cpp b/SubjuGator/perception/subjugator_pointcloud/src/Classification.cpp index 7ef99a2a0..ff5099fdc 100644 --- a/SubjuGator/perception/subjugator_pointcloud/src/Classification.cpp +++ b/SubjuGator/perception/subjugator_pointcloud/src/Classification.cpp @@ -63,7 +63,7 @@ cv::Point2d Classification::get_first_hit(cv::Mat &mat_ogrid, cv::Point2d start, break; mat_ogrid.at(cp_on_ray.y, cp_on_ray.x) = .5; } - // degrade things infront of the object as unoccupied + // degrade things in front of the object as unoccupied for (int j = 0; j < i; ++j) { cv::Point2d cp_on_ray = vec_d_theta * j + start; diff --git a/SubjuGator/simulation/subjugator_gazebo/src/subjugator_buoyancy.cc b/SubjuGator/simulation/subjugator_gazebo/src/subjugator_buoyancy.cc index adf27b153..903674653 100644 --- a/SubjuGator/simulation/subjugator_gazebo/src/subjugator_buoyancy.cc +++ b/SubjuGator/simulation/subjugator_gazebo/src/subjugator_buoyancy.cc @@ -80,7 +80,8 @@ void BuoyancyPlugin::Load(physics::ModelPtr _model, sdf::ElementPtr _sdf) if (this->volPropsMap.count(id) != 0) { - gzwarn << "Properties for link [" << name << "] already set, skipping " << "second property block" << std::endl; + gzwarn << "Properties for link [" << name << "] already set, skipping " + << "second property block" << std::endl; } if (linkElem->HasElement("center_of_volume")) diff --git a/SubjuGator/simulation/subjugator_gazebo/src/subjugator_test.cc b/SubjuGator/simulation/subjugator_gazebo/src/subjugator_test.cc index c81119d45..059a14583 100644 --- a/SubjuGator/simulation/subjugator_gazebo/src/subjugator_test.cc +++ b/SubjuGator/simulation/subjugator_gazebo/src/subjugator_test.cc @@ -16,12 +16,13 @@ class WorldPluginTutorial : public WorldPlugin // Make sure the ROS node for Gazebo has already been initialized if (!ros::isInitialized()) { - ROS_FATAL_STREAM("A ROS node for Gazebo has not been initialized, unable to load plugin. " << "Load the Gazebo " - "system plugin " - "'libgazebo_ros_" - "api_plugin.so' in " - "the gazebo_ros " - "package)"); + ROS_FATAL_STREAM("A ROS node for Gazebo has not been initialized, unable to load plugin. " + << "Load the Gazebo " + "system plugin " + "'libgazebo_ros_" + "api_plugin.so' in " + "the gazebo_ros " + "package)"); return; } diff --git a/mil_common/drivers/mil_usb_to_can/mil_usb_to_can/sub8/board.py b/mil_common/drivers/mil_usb_to_can/mil_usb_to_can/sub8/board.py index 285332e32..8b4b04829 100755 --- a/mil_common/drivers/mil_usb_to_can/mil_usb_to_can/sub8/board.py +++ b/mil_common/drivers/mil_usb_to_can/mil_usb_to_can/sub8/board.py @@ -41,7 +41,7 @@ def __init__(self, port: str, baud: int = 9600, simulated: bool = False, **kwarg def read_packet(self) -> Optional[ReceivePacket]: """ Read a packet from the board, if available. Returns a :class:`ReceivePacket` - instance if one was succefully read, or ``None`` if the in buffer is empty. + instance if one was successfully read, or ``None`` if the in buffer is empty. Returns: Optional[:class:`ReceivePacket`]: The packet, if found, otherwise ``None``. diff --git a/mil_common/gnc/odom_estimator/data/readme.txt b/mil_common/gnc/odom_estimator/data/readme.txt index e553912ca..2736cf68e 100644 --- a/mil_common/gnc/odom_estimator/data/readme.txt +++ b/mil_common/gnc/odom_estimator/data/readme.txt @@ -1,4 +1,4 @@ -when replaceing the WMM.COF file, make sure to convert it to unix style line ends. +when replacing the WMM.COF file, make sure to convert it to unix style line ends. Usually the file you can down load from https://www.ngdc.noaa.gov/geomag/WMM/soft.shtml will come with carriage return characters (for windows users). 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 7d670a0d3..750435fcb 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,7 +71,9 @@ 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(); @@ -119,7 +121,9 @@ 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(); @@ -168,7 +172,9 @@ 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) @@ -196,7 +202,9 @@ 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; @@ -551,8 +559,10 @@ 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/utils/mil_tools/nodes/tf_fudger.py b/mil_common/utils/mil_tools/nodes/tf_fudger.py index b526927f6..8d0b3bc03 100755 --- a/mil_common/utils/mil_tools/nodes/tf_fudger.py +++ b/mil_common/utils/mil_tools/nodes/tf_fudger.py @@ -179,7 +179,7 @@ def reset(): tf_line_to_add = tf_line.format(child=args.tf_child, p=p, q=np.round(q, 5), parent=args.tf_parent, prd="{prd}") for i,line in enumerate(lines): if args.tf_child in line and args.tf_parent in line: - tab_level = line[:line.find("<")] # The labs infront of the tf line + tab_level = line[:line.find("<")] # The labs in front of the tf line prd = int([x for x in line.split('"')[-2].split(" ") if x != ''][-1]) # Get the period from the tf line lines[i] = tab_level + tf_line_to_add.format(prd=prd) @@ -191,7 +191,7 @@ def reset(): elif 'pkg="tf"' in line: # In case we don't find the tf line of interest to change, we want to insert the new tf line after # the last tf line - tab_level = line[:line.find("<")] # The labs infront of the tf line + tab_level = line[:line.find("<")] # The labs in front of the tf line last_static_pub = i else: From fa15e29a62489971ce90f5aa848dc34f1cc359d3 Mon Sep 17 00:00:00 2001 From: cameron brown <52760912+cbrxyz@users.noreply.github.com> Date: Sat, 8 Jun 2024 18:22:25 -0400 Subject: [PATCH 2/4] Add C++ library for `mil_missions` (#1204) * Add C++ library for * clang-format --- mil_common/mil_missions/CMakeLists.txt | 34 ++++++++- mil_common/mil_missions/include/client.h | 33 +++++++++ .../include/mil_missions/client.h | 34 +++++++++ mil_common/mil_missions/package.xml | 2 + mil_common/mil_missions/src/client.cpp | 59 +++++++++++++++ .../mil_missions/test/run_mission_cpp.test | 7 ++ .../{run_mission.test => run_mission_py.test} | 2 +- .../mil_missions/test/test_run_mission.cpp | 72 +++++++++++++++++++ .../mil_missions/test/test_run_mission.py | 4 +- 9 files changed, 241 insertions(+), 6 deletions(-) create mode 100644 mil_common/mil_missions/include/client.h create mode 100644 mil_common/mil_missions/include/mil_missions/client.h create mode 100644 mil_common/mil_missions/src/client.cpp create mode 100644 mil_common/mil_missions/test/run_mission_cpp.test rename mil_common/mil_missions/test/{run_mission.test => run_mission_py.test} (93%) create mode 100644 mil_common/mil_missions/test/test_run_mission.cpp diff --git a/mil_common/mil_missions/CMakeLists.txt b/mil_common/mil_missions/CMakeLists.txt index b56124214..0561e0f66 100644 --- a/mil_common/mil_missions/CMakeLists.txt +++ b/mil_common/mil_missions/CMakeLists.txt @@ -1,20 +1,48 @@ cmake_minimum_required(VERSION 3.0.2) project(mil_missions) -find_package(catkin REQUIRED genmsg actionlib_msgs actionlib) +SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -g -pedantic -Wall -std=c++11") + +find_package(catkin REQUIRED rostest genmsg actionlib_msgs roscpp actionlib) catkin_python_setup() add_action_files( FILES DoMission.action ) + generate_messages( DEPENDENCIES std_msgs actionlib_msgs ) -catkin_package() +catkin_package( + INCLUDE_DIRS + include + LIBRARIES + mil_missions + CATKIN_DEPENDS + actionlib_msgs + roscpp +) + +include_directories( + include + SYSTEM + ${catkin_INCLUDE_DIRS} +) if(CATKIN_ENABLE_TESTING) find_package(rostest REQUIRED) add_rostest(test/import_mil_missions_examples.test) - add_rostest(test/run_mission.test) + add_rostest(test/run_mission_py.test) + add_rostest_gtest(run_test_mission_cpp + test/run_mission_cpp.test + test/test_run_mission.cpp + src/client.cpp) + target_link_libraries(run_test_mission_cpp ${catkin_LIBRARIES}) endif() + +add_library(mil_missions + src/client.cpp +) +target_include_directories(mil_missions PUBLIC include) +target_link_libraries(mil_missions ${catkin_LIBRARIES}) diff --git a/mil_common/mil_missions/include/client.h b/mil_common/mil_missions/include/client.h new file mode 100644 index 000000000..2bb6d9af7 --- /dev/null +++ b/mil_common/mil_missions/include/client.h @@ -0,0 +1,33 @@ +#pragma once + +#include +#include +#include +#include + +#include +#include + +namespace mil_missions +{ +class MissionClient +{ +public: + MissionClient(ros::NodeHandle &nh); + + // About missions + std::vector available_missions(); + + // Execute missions + bool wait_for_server(const ros::Duration timeout = ros::Duration(5.0)); + bool wait_for_result(const ros::Duration timeout = ros::Duration(0)); + DoMissionResult::ConstPtr get_result(); + actionlib::SimpleClientGoalState get_state(); + void run_mission(std::string mission_name, std::string parameters = ""); + void cancel_mission(); + +private: + actionlib::SimpleActionClient ac; + ros::NodeHandle _nh; +}; +} // namespace mil_missions diff --git a/mil_common/mil_missions/include/mil_missions/client.h b/mil_common/mil_missions/include/mil_missions/client.h new file mode 100644 index 000000000..912498915 --- /dev/null +++ b/mil_common/mil_missions/include/mil_missions/client.h @@ -0,0 +1,34 @@ +#pragma once + +#include +#include +#include +#include + +#include +#include + +namespace mil_missions +{ +class MissionClient +{ +public: + MissionClient(ros::NodeHandle &nh); + + // About missions + std::vector available_missions(); + + // Execute missions + bool wait_for_server(const ros::Duration timeout = ros::Duration(5.0)); + bool wait_for_result(const ros::Duration timeout = ros::Duration(0)); + DoMissionResult::ConstPtr get_result(); + actionlib::SimpleClientGoalState get_state(); + void run_mission(std::string mission_name, std::string parameters = ""); + void cancel_mission(); + +private: + actionlib::SimpleActionClient ac; + ros::NodeHandle _nh; +}; +} // namespace mil_missions +// #include "../../src/client.cpp" diff --git a/mil_common/mil_missions/package.xml b/mil_common/mil_missions/package.xml index 1d04f74a1..b32438f18 100644 --- a/mil_common/mil_missions/package.xml +++ b/mil_common/mil_missions/package.xml @@ -8,7 +8,9 @@ catkin actionlib actionlib_msgs + roscpp actionlib + roscpp actionlib_msgs python_qt_binding qt_gui.plugin diff --git a/mil_common/mil_missions/src/client.cpp b/mil_common/mil_missions/src/client.cpp new file mode 100644 index 000000000..afd07ad45 --- /dev/null +++ b/mil_common/mil_missions/src/client.cpp @@ -0,0 +1,59 @@ +#include + +namespace mil_missions +{ +MissionClient::MissionClient(ros::NodeHandle &nh) : ac("/mission", true), _nh(nh) +{ +} + +std::vector MissionClient::available_missions() +{ + std::vector missions; + if (!_nh.getParam("/available_missions", missions)) + { + ROS_ERROR("No available missions found"); + return {}; + } + return missions; +} + +void MissionClient::run_mission(std::string mission_name, std::string parameters) +{ + if (!ac.waitForServer(ros::Duration(5.0))) + { + ROS_ERROR("Could not connect to mission server to run mission"); + return; + } + + mil_missions::DoMissionGoal goal; + goal.mission = mission_name; + goal.parameters = parameters; + + ac.sendGoal(goal); +} + +void MissionClient::cancel_mission() +{ + ac.cancelAllGoals(); +} + +bool MissionClient::wait_for_server(ros::Duration timeout) +{ + return ac.waitForServer(timeout); +} + +bool MissionClient::wait_for_result(ros::Duration timeout) +{ + return ac.waitForResult(timeout); +} + +DoMissionResult::ConstPtr MissionClient::get_result() +{ + return ac.getResult(); +} + +actionlib::SimpleClientGoalState MissionClient::get_state() +{ + return ac.getState(); +} +} // namespace mil_missions diff --git a/mil_common/mil_missions/test/run_mission_cpp.test b/mil_common/mil_missions/test/run_mission_cpp.test new file mode 100644 index 000000000..688cd22d0 --- /dev/null +++ b/mil_common/mil_missions/test/run_mission_cpp.test @@ -0,0 +1,7 @@ + + + + + + + diff --git a/mil_common/mil_missions/test/run_mission.test b/mil_common/mil_missions/test/run_mission_py.test similarity index 93% rename from mil_common/mil_missions/test/run_mission.test rename to mil_common/mil_missions/test/run_mission_py.test index db3ec82d8..deee3ad5b 100644 --- a/mil_common/mil_missions/test/run_mission.test +++ b/mil_common/mil_missions/test/run_mission_py.test @@ -3,5 +3,5 @@ - + diff --git a/mil_common/mil_missions/test/test_run_mission.cpp b/mil_common/mil_missions/test/test_run_mission.cpp new file mode 100644 index 000000000..5e08fa0ed --- /dev/null +++ b/mil_common/mil_missions/test/test_run_mission.cpp @@ -0,0 +1,72 @@ +#include +#include +#include +#include +#include + +#include + +using namespace mil_missions; + +class MissionClientTest : public ::testing::Test +{ +protected: + MissionClientTest() : client(nh) + { + } + + ros::NodeHandle nh; + mil_missions::MissionClient client; +}; + +TEST_F(MissionClientTest, RunMission) +{ + EXPECT_TRUE(client.wait_for_server()); + + // Test running a mission + client.run_mission("PrintAndWait"); + EXPECT_TRUE(client.wait_for_result(ros::Duration(5.0))); + mil_missions::DoMissionResult::ConstPtr result = client.get_result(); + actionlib::SimpleClientGoalState state = client.get_state(); + EXPECT_EQ(state, actionlib::SimpleClientGoalState::SUCCEEDED); + EXPECT_TRUE(result->success); + EXPECT_EQ(result->parameters, ""); + EXPECT_EQ(result->result, "The darkness isn't so scary"); +} + +TEST_F(MissionClientTest, FailingMission) +{ + EXPECT_TRUE(client.wait_for_server()); + + // Test running a mission + client.run_mission("FailingMission"); + EXPECT_TRUE(client.wait_for_result(ros::Duration(5.0))); + mil_missions::DoMissionResult::ConstPtr result = client.get_result(); + actionlib::SimpleClientGoalState state = client.get_state(); + EXPECT_EQ(state, actionlib::SimpleClientGoalState::ABORTED); + EXPECT_FALSE(result->success); + EXPECT_TRUE(!result->parameters.empty()); + EXPECT_EQ(result->result, "Exception occurred in FailingMission mission: ValueError: This is an example error!"); +} + +TEST_F(MissionClientTest, CancelledTest) +{ + EXPECT_TRUE(client.wait_for_server()); + + // Test running a mission + client.run_mission("CancelledMission"); + EXPECT_TRUE(client.wait_for_result(ros::Duration(5.0))); + mil_missions::DoMissionResult::ConstPtr result = client.get_result(); + actionlib::SimpleClientGoalState state = client.get_state(); + EXPECT_EQ(state, actionlib::SimpleClientGoalState::ABORTED); + EXPECT_FALSE(result->success); + EXPECT_TRUE(result->parameters.empty()); + EXPECT_EQ(result->result, "mission cancelled"); +} + +int main(int argc, char **argv) +{ + testing::InitGoogleTest(&argc, argv); + ros::init(argc, argv, "run_mission_test_cpp"); + return RUN_ALL_TESTS(); +} diff --git a/mil_common/mil_missions/test/test_run_mission.py b/mil_common/mil_missions/test/test_run_mission.py index dfcb9d740..0895100a4 100755 --- a/mil_common/mil_missions/test/test_run_mission.py +++ b/mil_common/mil_missions/test/test_run_mission.py @@ -49,5 +49,5 @@ def test_cancelled_mission(self): if __name__ == "__main__": import rostest - rospy.init_node("run_mission_test") - rostest.rosrun("mil_missions", "run_mission", RunMissionTest) + rospy.init_node("run_mission_test_py") + rostest.rosrun("mil_missions", "run_missions_py", RunMissionTest) From ebf43b21d33e2aa2a706ac8106042a730e76bc79 Mon Sep 17 00:00:00 2001 From: cameron brown <52760912+cbrxyz@users.noreply.github.com> Date: Sat, 15 Jun 2024 18:30:21 -0400 Subject: [PATCH 3/4] Dynamic pressure, move thrusters from Fall 2023 (#1205) --- .../nav_tube_driver/src/nav_tube_driver.cpp | 26 ++++++++++++++++++- .../config/thruster_layout.yaml | 8 +++--- 2 files changed, 29 insertions(+), 5 deletions(-) diff --git a/SubjuGator/drivers/nav_tube_driver/src/nav_tube_driver.cpp b/SubjuGator/drivers/nav_tube_driver/src/nav_tube_driver.cpp index 5451370f3..a40080677 100644 --- a/SubjuGator/drivers/nav_tube_driver/src/nav_tube_driver.cpp +++ b/SubjuGator/drivers/nav_tube_driver/src/nav_tube_driver.cpp @@ -1,6 +1,8 @@ #include #include #include +#include +#include #include #include @@ -22,6 +24,8 @@ class NavTubeDriver ros::NodeHandle private_nh_; ros::Publisher pub_; + ros::Subscriber odom_sub_; + nav_msgs::Odometry recent_odom_msg_; std::string ip_; int port_; @@ -59,11 +63,13 @@ class NavTubeDriver ~NavTubeDriver(); void run(); + void odom_callback(const nav_msgs::OdometryConstPtr& msg); }; NavTubeDriver::NavTubeDriver(ros::NodeHandle nh, ros::NodeHandle private_nh) : nh_(nh), private_nh_(private_nh) { pub_ = nh.advertise("depth", 10); + odom_sub_ = nh.subscribe("odom", 10, &NavTubeDriver::odom_callback, this); ip_ = private_nh.param("ip", std::string("192.168.37.61")); port_ = private_nh.param("port", 33056); frame_id_ = private_nh.param("frame_id", "/depth"); @@ -85,6 +91,11 @@ NavTubeDriver::NavTubeDriver(ros::NodeHandle nh, ros::NodeHandle private_nh) : n reinterpret_cast(&heartbeat_packet[2])[0] = nw_ordering; } +void NavTubeDriver::odom_callback(const nav_msgs::OdometryConstPtr& ptr) +{ + recent_odom_msg_ = *ptr; +} + NavTubeDriver::~NavTubeDriver() { { @@ -194,7 +205,18 @@ void NavTubeDriver::read_messages(boost::shared_ptr socket) uint64_t bits = be64toh(*reinterpret_cast(&backing[2])); double pressure = *reinterpret_cast(&bits); - msg.depth = pressure; + if (recent_odom_msg_.header.seq) + { + // Accounts for the dynamic pressure applied to the pressure sensor + // when the sub is moving forwards or backwards + double velocity = recent_odom_msg_.twist.twist.linear.x; + double vel_effect = (abs(velocity) * velocity) / (1000 * 9.81); + msg.depth = pressure + vel_effect; + } + else + { + msg.depth = pressure; + } pub_.publish(msg); buffer = boost::asio::buffer(backing, sizeof(backing)); @@ -205,6 +227,8 @@ void NavTubeDriver::read_messages(boost::shared_ptr socket) buffer = boost::asio::buffer(buffer + bytes_read); prev = ros::Time::now(); + + ros::spinOnce(); } } diff --git a/SubjuGator/gnc/subjugator_thruster_mapper/config/thruster_layout.yaml b/SubjuGator/gnc/subjugator_thruster_mapper/config/thruster_layout.yaml index 48992559a..b18e4ac0d 100644 --- a/SubjuGator/gnc/subjugator_thruster_mapper/config/thruster_layout.yaml +++ b/SubjuGator/gnc/subjugator_thruster_mapper/config/thruster_layout.yaml @@ -32,7 +32,7 @@ thrusters: FLH: { node_id: 4, position: [0.2678, 0.2795, 0.0], - direction: [0.866, -0.5, 0.0], + direction: [-0.866, 0.5, 0.0], thrust_bounds: [-81.85, 99.7], calib: { # yamllint disable-line rule:line-length @@ -56,7 +56,7 @@ thrusters: FRH: { node_id: 0, position: [0.2678, -0.2795, 0.0], - direction: [0.866, 0.5, 0.0], + direction: [-0.866, -0.5, 0.0], thrust_bounds: [-81.85, 99.7], calib: { # yamllint disable-line rule:line-length @@ -80,7 +80,7 @@ thrusters: BLH: { node_id: 3, position: [-0.2678, 0.2795, 0.0], - direction: [0.866, 0.5, 0.0], + direction: [-0.866, -0.5, 0.0], thrust_bounds: [-81.85, 99.7], calib: { # yamllint disable-line rule:line-length @@ -104,7 +104,7 @@ thrusters: BRH: { node_id: 7, position: [-0.2678, -0.2795, 0.0], - direction: [0.866, -0.5, 0.0], + direction: [-0.866, 0.5, 0.0], thrust_bounds: [-81.85, 99.7], calib: { # yamllint disable-line rule:line-length From 99aa724b6c634ce15c60997e6dd7ba3f8ce3cd49 Mon Sep 17 00:00:00 2001 From: Cameron Brown Date: Mon, 17 Jun 2024 21:38:19 -0400 Subject: [PATCH 4/4] Fixing axros submodule --- mil_common/axros | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/mil_common/axros b/mil_common/axros index d0f12eedb..a86842c93 160000 --- a/mil_common/axros +++ b/mil_common/axros @@ -1 +1 @@ -Subproject commit d0f12eedb4622f5bbc5ba4fb63ae350f27a51f4b +Subproject commit a86842c93370a62c0dad3fba0e6ef250137edf21