From 5c15f16c495cf9c9c4f330deca18e541111258b0 Mon Sep 17 00:00:00 2001 From: Cameron Brown Date: Tue, 5 Nov 2024 12:13:29 -0500 Subject: [PATCH 1/4] pcodar: Add dynreconfig parameters for intensity filter bounds, add config files for real-life and sim --- .../navigator_launch/config/pcodar.yaml | 4 ++++ .../navigator_launch/config/pcodar_vrx.yaml | 8 +++++-- .../cfg/PCODAR.cfg | 4 ++++ .../pcodar_controller.hpp | 5 +++++ .../src/pcodar_controller.cpp | 22 ++++++++++++++----- 5 files changed, 35 insertions(+), 8 deletions(-) diff --git a/NaviGator/mission_control/navigator_launch/config/pcodar.yaml b/NaviGator/mission_control/navigator_launch/config/pcodar.yaml index cdde70798..f4ec88287 100644 --- a/NaviGator/mission_control/navigator_launch/config/pcodar.yaml +++ b/NaviGator/mission_control/navigator_launch/config/pcodar.yaml @@ -21,5 +21,9 @@ ogrid_width_meters: 500 ogrid_resolution_meters_per_cell: 0.3 ogrid_inflation_meters: 0.8 +# Intensity filter +intensity_filter_min_intensity: 10 +intensity_filter_max_intensity: 100 + # yamllint disable-line rule:line-length classifications: ["white_cylinder", "black_cylinder", "red_cylinder", "green_cylinder", "UNKNOWN"] diff --git a/NaviGator/mission_control/navigator_launch/config/pcodar_vrx.yaml b/NaviGator/mission_control/navigator_launch/config/pcodar_vrx.yaml index 8c50bb395..39c0d39fa 100644 --- a/NaviGator/mission_control/navigator_launch/config/pcodar_vrx.yaml +++ b/NaviGator/mission_control/navigator_launch/config/pcodar_vrx.yaml @@ -12,8 +12,8 @@ cluster_min_points: 5 cluster_max_points: 1000 # Associator -associator_max_distance: 2.0 -associator_forget_unseen: false +associator_max_distance: 20.0 +associator_forget_unseen: true # Ogrid ogrid_height_meters: 500 @@ -21,6 +21,10 @@ ogrid_width_meters: 500 ogrid_resolution_meters_per_cell: 0.3 ogrid_inflation_meters: 0.8 +# Intensity filter +intensity_filter_min_intensity: 0 +intensity_filter_max_intensity: 1 + # yamllint disable-line rule:line-length # classifications: ["buoy", "dock", "stc_platform", "red_totem", "green_totem", "blue_totem", "yellow_totem", "black_totem", "surmark46104", "surmark950400", "surmark950410", "UNKNOWN"] # yamllint disable-line rule:line-length diff --git a/mil_common/perception/point_cloud_object_detection_and_recognition/cfg/PCODAR.cfg b/mil_common/perception/point_cloud_object_detection_and_recognition/cfg/PCODAR.cfg index c0c5463ac..658a0bf15 100755 --- a/mil_common/perception/point_cloud_object_detection_and_recognition/cfg/PCODAR.cfg +++ b/mil_common/perception/point_cloud_object_detection_and_recognition/cfg/PCODAR.cfg @@ -26,4 +26,8 @@ gen.add("ogrid_width_meters", double_t, 16, "", 1000, 1, 10000) gen.add("ogrid_resolution_meters_per_cell", double_t, 16, "", 0.3, 0., 10.) gen.add("ogrid_inflation_meters", double_t, 16, "", 2, 0., 10.) +# Intensity filter +gen.add("intensity_filter_min_intensity", double_t, 32, "", 10.0, 0., 1000.) +gen.add("intensity_filter_max_intensity", double_t, 32, "", 100.0, 0., 1000.) + exit(gen.generate("point_cloud_object_detection_and_recognition", "pcodar", "PCODAR")) diff --git a/mil_common/perception/point_cloud_object_detection_and_recognition/include/point_cloud_object_detection_and_recognition/pcodar_controller.hpp b/mil_common/perception/point_cloud_object_detection_and_recognition/include/point_cloud_object_detection_and_recognition/pcodar_controller.hpp index 333b82b79..1db53eaa9 100644 --- a/mil_common/perception/point_cloud_object_detection_and_recognition/include/point_cloud_object_detection_and_recognition/pcodar_controller.hpp +++ b/mil_common/perception/point_cloud_object_detection_and_recognition/include/point_cloud_object_detection_and_recognition/pcodar_controller.hpp @@ -82,6 +82,10 @@ class NodeBase // Visualization MarkerManager marker_manager_; OgridManager ogrid_manager_; + + // Intensity filter + double intensity_filter_min_intensity; + double intensity_filter_max_intensity; }; class Node : public NodeBase @@ -96,6 +100,7 @@ class Node : public NodeBase private: bool bounds_update_cb(const mil_bounds::BoundsConfig& config) override; void ConfigCallback(Config const& config, uint32_t level) override; + void update_config(Config const& config); /// Reset PCODAR bool Reset(std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res) override; diff --git a/mil_common/perception/point_cloud_object_detection_and_recognition/src/pcodar_controller.cpp b/mil_common/perception/point_cloud_object_detection_and_recognition/src/pcodar_controller.cpp index 0c1ae8e7e..5a0ccf137 100644 --- a/mil_common/perception/point_cloud_object_detection_and_recognition/src/pcodar_controller.cpp +++ b/mil_common/perception/point_cloud_object_detection_and_recognition/src/pcodar_controller.cpp @@ -15,6 +15,8 @@ NodeBase::NodeBase(ros::NodeHandle _nh) , tf_listener(tf_buffer_, nh_) , global_frame_("enu") , config_server_(_nh) + , intensity_filter_min_intensity(10) + , intensity_filter_max_intensity(100) , objects_(std::make_shared()) { config_server_.setCallback(std::bind(&NodeBase::ConfigCallback, this, std::placeholders::_1, std::placeholders::_2)); @@ -125,6 +127,12 @@ Node::Node(ros::NodeHandle _nh) : NodeBase(_nh) input_cloud_filter_.set_robot_footprint(min, max); } +void Node::update_config(Config const& config) +{ + this->intensity_filter_min_intensity = config.intensity_filter_min_intensity; + this->intensity_filter_max_intensity = config.intensity_filter_max_intensity; +} + void Node::ConfigCallback(Config const& config, uint32_t level) { NodeBase::ConfigCallback(config, level); @@ -136,6 +144,8 @@ void Node::ConfigCallback(Config const& config, uint32_t level) detector_.update_config(config); if (!level || level & 8) ass.update_config(config); + if (!level || level & 32) + this->update_config(config); } void Node::initialize() @@ -171,14 +181,14 @@ void Node::velodyne_cb(const sensor_msgs::PointCloud2ConstPtr& pcloud) if (!transform_point_cloud(*pcloud, *pc)) return; - // our new filter vvv - pcl::PassThrough _temp_intensity_filter; - _temp_intensity_filter.setInputCloud(pc); - _temp_intensity_filter.setFilterFieldName("intensity"); - _temp_intensity_filter.setFilterLimits(10, 100); + // Intensity filter + pcl::PassThrough _intensity_filter; + _intensity_filter.setInputCloud(pc); + _intensity_filter.setFilterFieldName("intensity"); + _intensity_filter.setFilterLimits(this->intensity_filter_min_intensity, this->intensity_filter_max_intensity); point_cloud_ptr pc_without_i = boost::make_shared(); point_cloud_i_ptr pc_i_filtered = boost::make_shared(); - _temp_intensity_filter.filter(*pc_i_filtered); + _intensity_filter.filter(*pc_i_filtered); pc_without_i->points.resize(pc_i_filtered->size()); for (size_t i = 0; i < pc_i_filtered->points.size(); i++) From d55da12e7da27f3a096480025a8de21d6add2086 Mon Sep 17 00:00:00 2001 From: Cameron Brown Date: Tue, 5 Nov 2024 22:20:07 -0500 Subject: [PATCH 2/4] NaviGator/scripts: Add thruster spin script --- NaviGator/scripts/thruster_spin.py | 84 ++++++++++++++++++++++++++++++ 1 file changed, 84 insertions(+) create mode 100755 NaviGator/scripts/thruster_spin.py diff --git a/NaviGator/scripts/thruster_spin.py b/NaviGator/scripts/thruster_spin.py new file mode 100755 index 000000000..10fd850e3 --- /dev/null +++ b/NaviGator/scripts/thruster_spin.py @@ -0,0 +1,84 @@ +#!/usr/bin/env python3 + +import argparse +import threading + +import rospy +from roboteq_msgs.msg import Command + + +def publish_thruster_command(thruster, rate): + topic = f"/{thruster}_motor/cmd" + command = Command(mode=0, setpoint=float(rate)) + pub = rospy.Publisher(topic, Command, queue_size=10) + rate_obj = rospy.Rate(rate) + rospy.loginfo(f"Starting to publish to {topic} with setpoint: {rate}") + while not rospy.is_shutdown(): + pub.publish(command) + rate_obj.sleep() + + +def main(): + parser = argparse.ArgumentParser( + description="Spin thrusters for the autonomous boat", + ) + + parser.add_argument("--all", action="store_true", help="Spin all thrusters") + parser.add_argument("--rate", type=float, default=0, help="Set thruster spin rate") + parser.add_argument("--slow", action="store_true", help="Set slow rate (50)") + parser.add_argument("--medium", action="store_true", help="Set medium rate (100)") + parser.add_argument("--fast", action="store_true", help="Set fast rate (200)") + parser.add_argument( + "thrusters", + nargs="*", + help="List of thruster names to spin (FR, FL, BR, BL)", + ) + + args = parser.parse_args() + + # Define default rates based on arguments + if args.slow: + rate = 50 + elif args.medium: + rate = 100 + elif args.fast: + rate = 200 + elif args.rate > 0: + rate = args.rate + else: + rospy.logerr("No rate specified! Use --rate, --slow, --medium, or --fast.") + return + + # Initialize ROS node + rospy.init_node("thruster_spin_control", anonymous=True) + + # Determine thrusters to spin + thrusters = [] + if args.all: + thrusters = ["FR", "FL", "BR", "BL"] + elif args.thrusters: + thrusters = args.thrusters + else: + rospy.logerr( + "No thrusters specified! Use --all or list specific thrusters (FR, FL, BR, BL).", + ) + return + + # Start spinning each thruster in a separate thread + threads = [] + for thruster in thrusters: + rospy.loginfo(f"Starting {thruster} at rate {rate}") + thread = threading.Thread( + target=publish_thruster_command, + args=(thruster, rate), + ) + thread.start() + threads.append(thread) + + # Wait for all threads to complete + for thread in threads: + thread.join() + + +if __name__ == "__main__": + main() From 6f78b542f1325eae68742404d5cc477923182ef6 Mon Sep 17 00:00:00 2001 From: Cameron Brown Date: Thu, 7 Nov 2024 09:50:36 -0500 Subject: [PATCH 3/4] navigator_launch: Re-disable PCODAR forgetting and re-lower cluster tolerance --- .../mission_control/navigator_launch/config/pcodar_vrx.yaml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/NaviGator/mission_control/navigator_launch/config/pcodar_vrx.yaml b/NaviGator/mission_control/navigator_launch/config/pcodar_vrx.yaml index 39c0d39fa..b9cfb71e1 100644 --- a/NaviGator/mission_control/navigator_launch/config/pcodar_vrx.yaml +++ b/NaviGator/mission_control/navigator_launch/config/pcodar_vrx.yaml @@ -12,8 +12,8 @@ cluster_min_points: 5 cluster_max_points: 1000 # Associator -associator_max_distance: 20.0 -associator_forget_unseen: true +associator_max_distance: 2.0 +associator_forget_unseen: false # Ogrid ogrid_height_meters: 500 From d951d28397588e07401663755dc53dbc4fb2ab00 Mon Sep 17 00:00:00 2001 From: Cameron Brown Date: Thu, 7 Nov 2024 23:44:31 -0500 Subject: [PATCH 4/4] yolov7-ros: Add data yaml file for RobotX 2024 STC model file --- mil_common/perception/yolov7-ros | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/mil_common/perception/yolov7-ros b/mil_common/perception/yolov7-ros index 7ddd6b4f9..800224501 160000 --- a/mil_common/perception/yolov7-ros +++ b/mil_common/perception/yolov7-ros @@ -1 +1 @@ -Subproject commit 7ddd6b4f9d04fde7048ed409aa2fa97c522b9678 +Subproject commit 800224501ed4a9b7aa4ba71a2dbd67d59a3373b8