Enhancement/thruster viz asv (#18)

* added total force arrow

* made torque arc marker

* documentation

* cleanup

* resolved merge conflict

* reduced number of segments in arc

* cleanup

* removed hardcoded values

---------

Co-authored-by: Aldokan <[email protected]>

thruster-visualization/CMakeLists.txt

@@ -13,6 +13,7 @@ find_package(tf2 REQUIRED)
 find_package(tf2_ros REQUIRED)
 find_package(tf2_geometry_msgs REQUIRED)
 find_package(vortex_msgs REQUIRED)
+find_package(geometry_msgs REQUIRED)
 
 include_directories(include)
 
@@ -22,6 +23,7 @@ tf2_ros
 visualization_msgs
 tf2_geometry_msgs
 vortex_msgs
+geometry_msgs
 )
 
 add_executable(${PROJECT_NAME}_node 

thruster-visualization/include/thruster_visualization/thruster_broadcaster.hpp

@@ -28,6 +28,7 @@ class ThrusterVisualization : public rclcpp::Node {
          * @brief Constructor for ThrusterVisualization class.
          */
         explicit ThrusterVisualization();
+
     private:
         /**
          * @brief Publishes visualization_msg MarkerArray of type arrow according to the thruster positions, orientations and thruster forces data.
@@ -38,14 +39,63 @@ class ThrusterVisualization : public rclcpp::Node {
          * @brief Callback function for setting thruster data equal to the thruster forces message.
          * @param msg The thruster forces message.
          */
-        void topic_callback(const std_msgs::msg::Float32MultiArray &msg);
+        void thruster_forces_callback(const std_msgs::msg::Float32MultiArray &msg);
+
+        /**
+         * @brief Creates a marker representing the total force of the thrusters.
+         * @param total_force_magnitude The magnitude of the total force.
+         * @param total_force_orientation The orientation of the total force.
+         * @return The created marker.
+         */
+        visualization_msgs::msg::Marker create_total_force_marker(double total_force_magnitude, double total_force_orientation);
+
+        /**
+         * @brief Creates a marker representing an arc.
+         * @param frame_id The frame ID of the marker.
+         * @param id The ID of the marker.
+         * @param center The center point of the arc.
+         * @param radius The radius of the arc.
+         * @param start_angle The start angle of the arc.
+         * @param end_angle The end angle of the arc.
+         * @param num_segments The number of segments to approximate the arc.
+         * @return The created marker.
+         */
+        visualization_msgs::msg::Marker create_arc_marker(
+            const std::string& frame_id,
+            int id,
+            const geometry_msgs::msg::Point& center,
+            double radius,
+            double start_angle,
+            double end_angle,
+            int num_segments);
+
+        /**
+         * @brief Creates a marker representing the direction of the torque.
+         * @param frame_id The frame ID of the marker.
+         * @param id The ID of the marker.
+         * @param center The center point of the marker.
+         * @param radius The radius of the marker.
+         * @param final_angle The final angle of the marker.
+         * @param total_torque_z The total torque in the z-axis.
+         * @return The created marker.
+         */
+        visualization_msgs::msg::Marker create_torque_direction_marker(
+            const std::string& frame_id,
+            int id,
+            const geometry_msgs::msg::Point& center,
+            double radius,
+            double final_angle,
+            double total_torque_z);
 
-        rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr publisher_;
-        rclcpp::Subscription<std_msgs::msg::Float32MultiArray>::SharedPtr subscription_;
+        rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr thruster_marker_publisher_;
+        rclcpp::Subscription<std_msgs::msg::Float32MultiArray>::SharedPtr thruster_forces_subscriber_;
         rclcpp::TimerBase::SharedPtr timer_;
-        std::vector<std::array<float, 3>> thruster_positions_;
-        std::vector<float> thruster_orientations_;
+        std::vector<std::vector<double>> thruster_positions_;
+        std::vector<double> thruster_orientations_;
         std::vector<double> thruster_data_;
+        int num_thrusters_;
+        double total_force_magnitude_;
+        double total_force_orientation_;
 
 };
 

thruster-visualization/launch/thruster_visualization_asv.launch.py

@@ -0,0 +1,29 @@
+from math import pi
+from launch import LaunchDescription
+from launch_ros.actions import Node
+
+def generate_launch_description():
+    thruster_visualization_node = Node(
+            package='thruster_visualization',
+            executable='thruster_visualization_node',
+            name='thruster_visualization_node',
+            output='screen',
+            parameters=[{
+                'num_thrusters': 4,
+                #NOTE: These coordinates are in the base_link frame (NED)
+                'thruster0_position': [0.7, 0.5, 0.4],
+                'thruster0_orientation': 7.0 * pi / 4.0,
+
+                'thruster1_position': [-0.7, 0.5, 0.4],
+                'thruster1_orientation': 1.0 * pi / 4.0,
+
+                'thruster2_position': [-0.7, -0.5, 0.4],
+                'thruster2_orientation': 7.0 * pi / 4.0,
+
+                'thruster3_position': [0.7, -0.5, 0.4],
+                'thruster3_orientation': 1.0 * pi / 4.0,
+            }]
+        )
+    return LaunchDescription([
+        thruster_visualization_node
+    ])

thruster-visualization/package.xml

@@ -14,6 +14,7 @@
   <depend>tf2_ros</depend>
   <depend>tf2_geometry_msgs</depend>
   <depend>vortex_msgs</depend>
+  <depend>geometry_msgs</depend>
 
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>ament_lint_common</test_depend>