fixed merge conflicts

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"]

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

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(

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

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

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

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<ObjectMap>())
 {
   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<pointi_t> _temp_intensity_filter;
-  _temp_intensity_filter.setInputCloud(pc);
-  _temp_intensity_filter.setFilterFieldName("intensity");
-  _temp_intensity_filter.setFilterLimits(10, 100);
+  // Intensity filter
+  pcl::PassThrough<pointi_t> _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>();
   point_cloud_i_ptr pc_i_filtered = boost::make_shared<point_cloud_i>();
-  _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++)