Merge remote-tracking branch 'origin/main' into drill-update

# Conflicts:
#	kalman_gs/node_project/src/common/ros-interfaces.ts
#	kalman_gs/node_project/src/panels/index.ts
#	kalman_interfaces/msg/MasterMessage.msg
#	kalman_master/launch/master.launch.py

Author: michal-proc

kalman_arc/CMakeLists.txt

@@ -25,6 +25,15 @@ find_package(Qt5 REQUIRED COMPONENTS Widgets)
 find_package(qt_gui_cpp REQUIRED)
 find_package(rqt_gui_cpp REQUIRED)
 
+# darkest rock finder deps
+find_package(rclcpp_components REQUIRED)
+find_package(pcl_ros REQUIRED)
+find_package(pcl_conversions REQUIRED)
+find_package(vision_msgs REQUIRED)
+find_package(tf2_ros REQUIRED)
+find_package(tf2_geometry_msgs REQUIRED)
+find_package(std_srvs REQUIRED)
+
 add_library(rscp_proto_messages
   proto/rscp.proto
 )
@@ -87,8 +96,6 @@ target_include_directories(rqt_rscp
 )
 
 target_link_libraries(rqt_rscp
-  kalman_arc_cobs
-  rscp_proto_messages
   Qt5::Widgets
 )
 
@@ -97,8 +104,65 @@ ament_target_dependencies(rqt_rscp
   qt_gui_cpp
   rclcpp
   rqt_gui_cpp
-  std_msgs
+  kalman_interfaces
 )
+find_package(sensor_msgs REQUIRED)
+find_package(geometry_msgs REQUIRED)
+find_package(tf2_ros REQUIRED)
+find_package(tf2_sensor_msgs REQUIRED)
+find_package(grid_map_ros REQUIRED)
+find_package(grid_map_core REQUIRED)
+find_package(grid_map_msgs REQUIRED)
+find_package(rclcpp_components REQUIRED)
+find_package(std_srvs REQUIRED)
+
+add_library(peak_finder_node SHARED src/peak_finder_node.cpp)
+ament_target_dependencies(peak_finder_node
+  rclcpp
+  rclcpp_components
+  sensor_msgs
+  tf2_ros
+  tf2_sensor_msgs
+  grid_map_ros
+  grid_map_core
+  grid_map_msgs
+  geometry_msgs
+  std_srvs
+)
+rclcpp_components_register_node(
+  peak_finder_node
+  PLUGIN "kalman_arc::PeakFinder"
+  EXECUTABLE peak_finder
+)
+install(TARGETS peak_finder_node peak_finder
+  EXPORT kalman_arc-targets
+  RUNTIME DESTINATION bin
+  LIBRARY DESTINATION lib
+  ARCHIVE DESTINATION lib
+)
+
+# >>> darkest rock finder
+
+add_library(darkest_rock_finder_node SHARED src/darkest_rock_finder_node.cpp)
+ament_target_dependencies(darkest_rock_finder_node
+  rclcpp
+  rclcpp_components
+  pcl_ros
+  pcl_conversions
+  vision_msgs
+  tf2_ros
+  tf2_geometry_msgs
+  std_srvs
+  # This dependency is completely unnnecessary, but compilation fails without it. I don't know why. I don't think I want to know:
+  grid_map_core # 67
+)
+rclcpp_components_register_node(
+  darkest_rock_finder_node
+  PLUGIN "rock_finder::DarkestRockFinder"
+  EXECUTABLE darkest_rock_finder
+)
+
+# <<< darkest rock finder
 
 install(PROGRAMS
   scripts/travel_distance_meter
@@ -110,6 +174,7 @@ install(TARGETS
   rscp_proto_messages
   rscp_node
   rqt_rscp
+  darkest_rock_finder_node
   ARCHIVE DESTINATION lib
   LIBRARY DESTINATION lib
   RUNTIME DESTINATION lib/${PROJECT_NAME}

kalman_arc/include/kalman_arc/rqt_rscp.hpp

@@ -1,6 +1,8 @@
 #pragma once
 
 #include <QWidget>
+#include <kalman_interfaces/msg/arc_rscp_request.hpp>
+#include <kalman_interfaces/msg/detail/arc_rscp_request__struct.hpp>
 #include <rclcpp/publisher.hpp>
 #include <rqt_gui_cpp/plugin.h>
 #include <std_msgs/msg/u_int8_multi_array.hpp>
@@ -36,7 +38,7 @@ private slots:
 	void publish_request();
 
 private:
-	using UInt8MultiArray = std_msgs::msg::UInt8MultiArray;
+    using ArcRscpRequest = kalman_interfaces::msg::ArcRscpRequest;
 
 	QWidget        *widget_       = nullptr;
 	QComboBox      *request_type_ = nullptr;
@@ -48,7 +50,6 @@ private slots:
 	QDoubleSpinBox *altitude_     = nullptr;
 	QDoubleSpinBox *radius_       = nullptr;
 	QLabel         *status_       = nullptr;
-	QLabel         *bytes_        = nullptr;
 
 	QWidget *arm_row_       = nullptr;
 	QWidget *stage_row_     = nullptr;
@@ -57,7 +58,7 @@ private slots:
 	QWidget *altitude_row_  = nullptr;
 	QWidget *radius_row_    = nullptr;
 
-	rclcpp::Publisher<UInt8MultiArray>::SharedPtr publisher_;
+	rclcpp::Publisher<ArcRscpRequest>::SharedPtr publisher_;
 };
 
 } // namespace kalman_arc

kalman_arc/launch/arc.launch.py

@@ -21,7 +21,18 @@ def get_bool(name):
             name="travel_distance_meter_node",
             parameters=[{"deadzone": float(deadzone_val)}],
             output="screen",
-        )
+        ),
+        Node(
+            package="kalman_arc",
+            executable="darkest_rock_finder",
+        ),
+        Node(
+            package="kalman_arc",
+            executable="peak_finder",
+            name="peak_finder_node",
+            parameters=[{"map_resolution": 0.3}],
+            output="screen",
+        ),
     ]
 
     if get_bool("enable_rscp_hw_driver"):

kalman_arc/package.xml

@@ -13,6 +13,10 @@
   <depend>rclpy</depend>
   <depend>rclcpp</depend>
   <depend>std_msgs</depend>
+  <depend>rclcpp_components</depend>
+  <depend>sensor_msgs</depend>
+  <depend>message_filters</depend>
+  <depend>geometry_msgs</depend>
   <depend>nav_msgs</depend>
   <depend>kalman_interfaces</depend>
   <depend>pluginlib</depend>
@@ -23,6 +27,10 @@
   <build_depend>protobuf-dev</build_depend>
   <exec_depend>protobuf-dev</exec_depend>
   <build_depend>qtbase5-dev</build_depend>
+  <depend>tf2_ros</depend>
+  <depend>tf2_sensor_msgs</depend>
+  <depend>grid_map</depend>
+  <depend>std_srvs</depend>
 
   <exec_depend>ros2launch</exec_depend>