Walk_viz cleanup (#622)

bitbots_motion/bitbots_quintic_walk/include/bitbots_quintic_walk/walk_utils.hpp

@@ -13,7 +13,16 @@
 
 namespace bitbots_quintic_walk {
 
-enum WalkState { PAUSED, WALKING, IDLE, START_MOVEMENT, STOP_MOVEMENT, START_STEP, STOP_STEP, KICK };
+enum WalkState {
+  IDLE = 0,
+  START_MOVEMENT = 1,
+  START_STEP = 2,
+  WALKING = 3,
+  PAUSED = 4,
+  KICK = 5,
+  STOP_STEP = 6,
+  STOP_MOVEMENT = 7
+};
 
 struct WalkRequest {
   std::vector<double> linear_orders = {0, 0, 0};
@@ -106,4 +115,4 @@ inline void tf_pose_to_msg(tf2::Transform &tf_pose, geometry_msgs::msg::Pose &ms
 
 }  // namespace bitbots_quintic_walk
 
-#endif  // BITBOTS_QUINTIC_WALK_INCLUDE_BITBOTS_QUINTIC_WALK_WALK_UTILS_H_
+#endif  // BITBOTS_QUINTIC_WALK_INCLUDE_BITBOTS_QUINTIC_WALK_WALK_UTILS_H_

bitbots_motion/bitbots_quintic_walk/include/bitbots_quintic_walk/walk_visualizer.hpp

@@ -10,6 +10,7 @@
 #include <bitbots_quintic_walk/walk_engine.hpp>
 #include <bitbots_quintic_walk/walk_utils.hpp>
 #include <moveit_msgs/msg/robot_state.hpp>
+#include <ranges>
 #include <rclcpp/rclcpp.hpp>
 #include <tf2_eigen/tf2_eigen.hpp>
 #include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
@@ -24,26 +25,46 @@ class WalkVisualizer : public bitbots_splines::AbstractVisualizer {
  public:
   explicit WalkVisualizer(rclcpp::Node::SharedPtr node, walking::Params::Node::Tf tf_config);
 
-  void publishArrowMarker(std::string name_space, std::string frame, geometry_msgs::msg::Pose pose, float r, float g,
-                          float b, float a);
+  visualization_msgs::msg::Marker createArrowMarker(const std::string &name_space, const std::string &frame,
+                                                    const geometry_msgs::msg::Pose &pose,
+                                                    const std_msgs::msg::ColorRGBA &color);
 
-  void publishEngineDebug(WalkResponse response);
-  void publishIKDebug(WalkResponse response, moveit::core::RobotStatePtr current_state,
-                      bitbots_splines::JointGoals joint_goals);
-  void publishWalkMarkers(WalkResponse response);
+  std::pair<bitbots_quintic_walk::msg::WalkEngineDebug, visualization_msgs::msg::MarkerArray> buildEngineDebugMsgs(
+      WalkResponse response);
+  std::pair<bitbots_quintic_walk::msg::WalkDebug, visualization_msgs::msg::MarkerArray> buildIKDebugMsgs(
+      WalkResponse response, moveit::core::RobotStatePtr current_state, bitbots_splines::JointGoals joint_goals);
+  visualization_msgs::msg::MarkerArray buildWalkMarkers(WalkResponse response);
 
   void init(moveit::core::RobotModelPtr kinematic_model);
 
+  void publishDebug(const WalkResponse &current_response, const moveit::core::RobotStatePtr &current_state,
+                    const bitbots_splines::JointGoals &motor_goals);
+
+  std_msgs::msg::ColorRGBA colorFactory(double r, double g, double b) {
+    std_msgs::msg::ColorRGBA color;
+    color.r = r;
+    color.g = g;
+    color.b = b;
+    color.a = 1.0;
+    return color;
+  }
+
+  const std_msgs::msg::ColorRGBA BLACK = colorFactory(0.0, 0.0, 0.0);
+  const std_msgs::msg::ColorRGBA BLUE = colorFactory(0.0, 0.0, 1.0);
+  const std_msgs::msg::ColorRGBA GREEN = colorFactory(0.0, 1.0, 0.0);
+  const std_msgs::msg::ColorRGBA ORANGE = colorFactory(1.0, 0.5, 0.0);
+  const std_msgs::msg::ColorRGBA RED = colorFactory(1.0, 0.0, 0.0);
+  const std_msgs::msg::ColorRGBA WHITE = colorFactory(1.0, 1.0, 1.0);
+  const std_msgs::msg::ColorRGBA YELLOW = colorFactory(1.0, 1.0, 0.0);
+
  private:
   rclcpp::Node::SharedPtr node_;
 
   walking::Params::Node::Tf tf_config_;
 
-  int marker_id_ = 1;
-
   rclcpp::Publisher<bitbots_quintic_walk::msg::WalkDebug>::SharedPtr pub_debug_;
   rclcpp::Publisher<bitbots_quintic_walk::msg::WalkEngineDebug>::SharedPtr pub_engine_debug_;
-  rclcpp::Publisher<visualization_msgs::msg::Marker>::SharedPtr pub_debug_marker_;
+  rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr pub_debug_marker_;
 
   moveit::core::RobotModelPtr kinematic_model_;
 };

bitbots_motion/bitbots_quintic_walk/src/walk_node.cpp

@@ -172,11 +172,7 @@ void WalkNode::run() {
   }
 }
 
-void WalkNode::publish_debug() {
-  visualizer_.publishIKDebug(current_stabilized_response_, current_state_, motor_goals_);
-  visualizer_.publishWalkMarkers(current_stabilized_response_);
-  visualizer_.publishEngineDebug(current_response_);
-}
+void WalkNode::publish_debug() { visualizer_.publishDebug(current_stabilized_response_, current_state_, motor_goals_); }
 
 bitbots_msgs::msg::JointCommand WalkNode::step(double dt) {
   WalkRequest request(current_request_);