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..b9cfb71e1 100644 --- a/NaviGator/mission_control/navigator_launch/config/pcodar_vrx.yaml +++ b/NaviGator/mission_control/navigator_launch/config/pcodar_vrx.yaml @@ -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/NaviGator/mission_control/navigator_missions/navigator_missions/navigator.py b/NaviGator/mission_control/navigator_missions/navigator_missions/navigator.py index 59ea23f97..808a3c948 100644 --- a/NaviGator/mission_control/navigator_missions/navigator_missions/navigator.py +++ b/NaviGator/mission_control/navigator_missions/navigator_missions/navigator.py @@ -213,15 +213,21 @@ def enu_odom_set(odom): cls._grind_motor_pub.setup(), ) - cls._ball_spin_srv = cls.nh.get_service_client("/ball_launcher/spin", SetBool) - cls._ball_launch_srv = cls.nh.get_service_client( - "/ball_launcher/drop_ball", - Emptysrv, - ) - await asyncio.gather( - cls._ball_spin_srv.setup(), - cls._ball_launch_srv.setup(), - ) + try: + cls._ball_spin_srv = cls.nh.get_service_client( + "/ball_launcher/spin", + SetBool, + ) + cls._ball_launch_srv = cls.nh.get_service_client( + "/ball_launcher/drop_ball", + Emptysrv, + ) + except AttributeError as err: + fprint( + f"Error getting ball launcher service clients: {err}", + title="NAVIGATOR", + msg_color="red", + ) try: cls._actuator_client = cls.nh.get_service_client( 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() 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 c65fa317d..a4817b249 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 @@ -88,6 +88,9 @@ class NodeBase OgridManager ogrid_manager_; Config saved_config_; + // Intensity filter + double intensity_filter_min_intensity; + double intensity_filter_max_intensity; }; class Node : public NodeBase @@ -104,6 +107,7 @@ class Node : public NodeBase void restore_config() override; 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; bool StoreParameters(std_srvs::SetBool::Request& req, std_srvs::SetBool::Response& res); 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 00b987cd7..9b32ab50a 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)); @@ -166,6 +168,12 @@ bool Node::StoreParameters(std_srvs::SetBool::Request& req, std_srvs::SetBool::R return true; } +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); @@ -177,6 +185,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() @@ -213,14 +223,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++)