Cleanup lowlevel

.git-blame-ignore-revs

@@ -2,3 +2,6 @@
 a5a1b5cb51afa766e7c9f72fbba2f79a858641c2
 975eb13a66bcf2e50525886453b40dd48b3b168f
 874863ef235c3713aa1f50b507cb9017afdba0f5
+45e252ecd15849f7794de05dae7cbea0382a48a1
+ba13aadddd9356fc92d6fb998aafe59c485b468b
+45e252ecd15849f7794de05dae7cbea0382a48a1

bitbots_buttons/CMakeLists.txt

@@ -12,7 +12,6 @@ find_package(backward_ros REQUIRED)
 find_package(bitbots_docs REQUIRED)
 find_package(bitbots_msgs REQUIRED)
 find_package(bitbots_localization REQUIRED)
-find_package(humanoid_league_msgs REQUIRED)
 find_package(rclcpp REQUIRED)
 find_package(std_srvs REQUIRED)
 find_package(test_msgs REQUIRED)
@@ -23,7 +22,6 @@ ament_target_dependencies(button_node
         backward_ros
         bitbots_msgs
         bitbots_localization
-        humanoid_league_msgs
         rclcpp
         std_msgs
         std_srvs
@@ -41,7 +39,6 @@ ament_export_dependencies(std_msgs)
 ament_export_dependencies(ament_cmake)
 ament_export_dependencies(bitbots_docs)
 ament_export_dependencies(bitbots_msgs)
-ament_export_dependencies(humanoid_league_msgs)
 ament_export_dependencies(std_msgs)
 ament_export_include_directories(${INCLUDE_DIRS})
 

bitbots_buttons/package.xml

@@ -17,9 +17,8 @@
 
   <depend>backward_ros</depend>
   <depend>bitbots_docs</depend>
-  <depend>bitbots_msgs</depend>
   <depend>bitbots_localization</depend>
-  <depend>humanoid_league_msgs</depend>
+  <depend>bitbots_msgs</depend>
   <depend>rclcpp</depend>
   <depend>std_msgs</depend>
   <depend>std_srvs</depend>

bitbots_buttons/src/button_node.cpp

@@ -1,9 +1,9 @@
 #include <signal.h>
 
 #include <bitbots_localization/srv/reset_filter.hpp>
+#include <bitbots_msgs/msg/audio.hpp>
 #include <bitbots_msgs/msg/buttons.hpp>
 #include <bitbots_msgs/srv/manual_penalize.hpp>
-#include <humanoid_league_msgs/msg/audio.hpp>
 #include <rclcpp/experimental/executors/events_executor/events_executor.hpp>
 #include <rclcpp/rclcpp.hpp>
 #include <std_msgs/msg/bool.hpp>
@@ -41,7 +41,7 @@ class ButtonNode : public rclcpp::Node {
     button3_time_ = 0.0;
 
     // --- Initialize Topics ---
-    speak_pub_ = this->create_publisher<humanoid_league_msgs::msg::Audio>("/speak", 1);
+    speak_pub_ = this->create_publisher<bitbots_msgs::msg::Audio>("/speak", 1);
     shoot_publisher_ = this->create_publisher<std_msgs::msg::Bool>("/shoot_button", 1);
 
     if (manual_penalty_mode_) {
@@ -176,7 +176,7 @@ class ButtonNode : public rclcpp::Node {
      *  Helper method to send a message for text-to-speech output
      */
     if (speak_) {
-      humanoid_league_msgs::msg::Audio msg = humanoid_league_msgs::msg::Audio();
+      bitbots_msgs::msg::Audio msg = bitbots_msgs::msg::Audio();
       msg.text = text;
       msg.priority = 100;
       speak_pub_->publish(msg);
@@ -199,7 +199,7 @@ class ButtonNode : public rclcpp::Node {
   double button2_time_;
   double button3_time_;
 
-  rclcpp::Publisher<humanoid_league_msgs::msg::Audio>::SharedPtr speak_pub_;
+  rclcpp::Publisher<bitbots_msgs::msg::Audio>::SharedPtr speak_pub_;
   rclcpp::Publisher<std_msgs::msg::Bool>::SharedPtr shoot_publisher_;
   rclcpp::Client<bitbots_msgs::srv::ManualPenalize>::SharedPtr manual_penalize_client_;
   rclcpp::Client<std_srvs::srv::Empty>::SharedPtr foot_zero_client_;

bitbots_ros_control/CMakeLists.txt

@@ -16,7 +16,6 @@ find_package(controller_manager REQUIRED)
 find_package(diagnostic_msgs REQUIRED)
 find_package(dynamixel_workbench_toolbox REQUIRED)
 find_package(hardware_interface REQUIRED)
-find_package(humanoid_league_msgs REQUIRED)
 find_package(pluginlib REQUIRED)
 find_package(rclcpp REQUIRED)
 find_package(realtime_tools REQUIRED)
@@ -62,7 +61,6 @@ ament_target_dependencies(ros_control
         diagnostic_msgs
         dynamixel_workbench_toolbox
         hardware_interface
-        humanoid_league_msgs
         pluginlib
         rclcpp
         realtime_tools
@@ -85,7 +83,6 @@ ament_target_dependencies(pressure_converter
         diagnostic_msgs
         dynamixel_workbench_toolbox
         hardware_interface
-        humanoid_league_msgs
         pluginlib
         rclcpp
         realtime_tools
@@ -105,7 +102,6 @@ ament_export_dependencies(controller_interface)
 ament_export_dependencies(controller_manager)
 ament_export_dependencies(dynamixel_workbench_toolbox)
 ament_export_dependencies(hardware_interface)
-ament_export_dependencies(humanoid_league_msgs)
 ament_export_dependencies(pluginlib)
 ament_export_dependencies(realtime_tools)
 ament_export_dependencies(std_msgs)

bitbots_ros_control/config/analyzers.yaml

@@ -8,12 +8,6 @@ analyzers:
       startswith: [ 'DS']
       timeout: 0.2
       find_and_remove_prefix: ['DS']
-    #Pressure:
-    #  type: diagnostic_aggregator/GenericAnalyzer
-    #  path: Pressure
-    #  startswith: ['PS']
-    #  timeout: 0.2
-    #  find_and_remove_prefix: ['PS']
     Core:
       type: diagnostic_aggregator/GenericAnalyzer
       path: Core
@@ -43,4 +37,10 @@ analyzers:
       path: System
       startswith: ['SYSTEM']
       timeout: 1.0
-      find_and_remove_prefix: ['SYSTEM']
+      find_and_remove_prefix: ['SYSTEM']
+    #Pressure:
+    #  type: diagnostic_aggregator/GenericAnalyzer
+    #  path: Pressure
+    #  startswith: ['PS']
+    #  timeout: 0.2
+    #  find_and_remove_prefix: ['PS']

bitbots_ros_control/include/bitbots_ros_control/bitfoot_hardware_interface.hpp

@@ -7,7 +7,6 @@
 #include <bitbots_ros_control/hardware_interface.hpp>
 #include <diagnostic_msgs/msg/diagnostic_array.hpp>
 #include <diagnostic_msgs/msg/diagnostic_status.hpp>
-#include <humanoid_league_msgs/msg/audio.hpp>
 #include <rclcpp/rclcpp.hpp>
 #include <string>
 

bitbots_ros_control/include/bitbots_ros_control/button_hardware_interface.hpp

@@ -9,7 +9,6 @@
 #include <diagnostic_msgs/msg/diagnostic_array.hpp>
 #include <diagnostic_msgs/msg/diagnostic_status.hpp>
 #include <hardware_interface/sensor_interface.hpp>
-#include <humanoid_league_msgs/msg/audio.hpp>
 #include <rclcpp/rclcpp.hpp>
 #include <string>
 

bitbots_ros_control/include/bitbots_ros_control/core_hardware_interface.hpp

@@ -36,9 +36,6 @@ class CoreHardwareInterface : public bitbots_ros_control::HardwareInterface {
 
   std::shared_ptr<DynamixelDriver> driver_;
 
-  // always keep the lasts values to check if they different
-  std::vector<std::vector<double>> current_pressure_;
-
   int id_;
   int read_rate_;
   int read_counter_;

bitbots_ros_control/include/bitbots_ros_control/dynamixel_servo_hardware_interface.hpp

@@ -11,7 +11,6 @@
 #include <bitset>
 #include <diagnostic_msgs/msg/diagnostic_array.hpp>
 #include <diagnostic_msgs/msg/diagnostic_status.hpp>
-#include <humanoid_league_msgs/msg/audio.hpp>
 #include <rclcpp/rclcpp.hpp>
 #include <sensor_msgs/msg/joint_state.hpp>
 #include <std_msgs/msg/bool.hpp>

bitbots_ros_control/include/bitbots_ros_control/imu_hardware_interface.hpp

@@ -77,6 +77,12 @@ class ImuHardwareInterface : public bitbots_ros_control::HardwareInterface {
   rclcpp::Service<bitbots_msgs::srv::SetAccelerometerCalibrationThreshold>::SharedPtr
       set_accel_calib_threshold_service_;
 
+  rclcpp::Publisher<sensor_msgs::msg::Imu>::SharedPtr imu_pub_;
+  rclcpp::Publisher<diagnostic_msgs::msg::DiagnosticArray>::SharedPtr diagnostic_pub_;
+  sensor_msgs::msg::Imu imu_msg_;
+
+  int diag_counter_;
+
   void setIMURanges(const std::shared_ptr<bitbots_msgs::srv::IMURanges::Request> req,
                     std::shared_ptr<bitbots_msgs::srv::IMURanges::Response> resp);
   void calibrateGyro(const std::shared_ptr<std_srvs::srv::Empty::Request> req,
@@ -94,10 +100,6 @@ class ImuHardwareInterface : public bitbots_ros_control::HardwareInterface {
   void setAccelCalibrationThreshold(
       const std::shared_ptr<bitbots_msgs::srv::SetAccelerometerCalibrationThreshold::Request> req,
       std::shared_ptr<bitbots_msgs::srv::SetAccelerometerCalibrationThreshold::Response> resp);
-  rclcpp::Publisher<sensor_msgs::msg::Imu>::SharedPtr imu_pub_;
-  rclcpp::Publisher<diagnostic_msgs::msg::DiagnosticArray>::SharedPtr diagnostic_pub_;
-  sensor_msgs::msg::Imu imu_msg_;
-  int diag_counter_;
 };
 }  // namespace bitbots_ros_control
 #endif

bitbots_ros_control/include/bitbots_ros_control/leds_hardware_interface.hpp

@@ -40,4 +40,4 @@ class LedsHardwareInterface : public bitbots_ros_control::HardwareInterface {
   rclcpp::Subscription<std_msgs::msg::ColorRGBA>::SharedPtr sub2_;
 };
 }  // namespace bitbots_ros_control
-#endif
+#endif

bitbots_ros_control/include/bitbots_ros_control/pressure_converter.hpp

@@ -26,10 +26,10 @@ class PressureConverter {
   rclcpp::Publisher<bitbots_msgs::msg::FootPressure>::SharedPtr filtered_pub_;
   rclcpp::Publisher<geometry_msgs::msg::PointStamped>::SharedPtr cop_pub_;
   std::vector<rclcpp::Publisher<geometry_msgs::msg::WrenchStamped>::SharedPtr> wrench_pubs_;
-  std::vector<std::string> wrench_frames_;
   rclcpp::Subscription<bitbots_msgs::msg::FootPressure>::SharedPtr sub_;
   std::unique_ptr<tf2_ros::TransformBroadcaster> tf_broadcaster_;
 
+  std::vector<std::string> wrench_frames_;
   std::vector<double> zero_, scale_;
   std::vector<std::vector<double>> previous_values_, zero_and_scale_values_;
   bool save_zero_and_scale_values_;
@@ -38,10 +38,12 @@ class PressureConverter {
   double cop_threshold_;
   char side_;
   std::string req_type_;
-  std::shared_ptr<bitbots_msgs::srv::FootScale::Request> request_;
   std::string scale_lr_, zero_lr_, cop_lr_, sole_lr_;
+  std::shared_ptr<bitbots_msgs::srv::FootScale::Request> request_;
+
   rclcpp::Service<std_srvs::srv::Empty>::SharedPtr zero_service_;
   rclcpp::Service<bitbots_msgs::srv::FootScale>::SharedPtr scale_service_;
+
   void pressureCallback(bitbots_msgs::msg::FootPressure pressure_raw);
   void resetZeroAndScaleValues();
   bool zeroCallback(const std::shared_ptr<std_srvs::srv::Empty::Request> req,

bitbots_ros_control/include/bitbots_ros_control/servo_bus_interface.hpp

@@ -3,13 +3,13 @@
 
 #include <dynamixel_driver.h>
 
+#include <bitbots_msgs/msg/audio.hpp>
 #include <bitbots_msgs/msg/joint_torque.hpp>
 #include <bitbots_ros_control/hardware_interface.hpp>
 #include <bitbots_ros_control/utils.hpp>
 #include <bitset>
 #include <diagnostic_msgs/msg/diagnostic_array.hpp>
 #include <diagnostic_msgs/msg/diagnostic_status.hpp>
-#include <humanoid_league_msgs/msg/audio.hpp>
 #include <rcl_interfaces/msg/list_parameters_result.hpp>
 #include <rclcpp/rclcpp.hpp>
 #include <sensor_msgs/msg/joint_state.hpp>
@@ -122,7 +122,7 @@ class ServoBusInterface : public bitbots_ros_control::HardwareInterface {
   int reading_errors_;
   int reading_successes_;
   rclcpp::Publisher<diagnostic_msgs::msg::DiagnosticArray>::SharedPtr diagnostic_pub_;
-  rclcpp::Publisher<humanoid_league_msgs::msg::Audio>::SharedPtr speak_pub_;
+  rclcpp::Publisher<bitbots_msgs::msg::Audio>::SharedPtr speak_pub_;
 };
 }  // namespace bitbots_ros_control
 #endif  // BITBOTS_ROS_CONTROL_INCLUDE_BITBOTS_ROS_CONTROL_SERVO_BUS_INTERFACE_H_

bitbots_ros_control/include/bitbots_ros_control/utils.hpp

@@ -1,19 +1,19 @@
 #ifndef BITBOTS_ROS_CONTROL_INCLUDE_BITBOTS_ROS_CONTROL_UTILS_H_
 #define BITBOTS_ROS_CONTROL_INCLUDE_BITBOTS_ROS_CONTROL_UTILS_H_
 
-#include "humanoid_league_msgs/msg/audio.hpp"
+#include "bitbots_msgs/msg/audio.hpp"
 #include "rclcpp/rclcpp.hpp"
 
 namespace bitbots_ros_control {
 
 enum ControlMode { POSITION_CONTROL, VELOCITY_CONTROL, EFFORT_CONTROL, CURRENT_BASED_POSITION_CONTROL };
 
-bool stringToControlMode(rclcpp::Node::SharedPtr nh, const std::string &control_modestr, ControlMode &control_mode);
-void speakError(rclcpp::Publisher<humanoid_league_msgs::msg::Audio>::SharedPtr speak_pub, const std::string &text);
+bool stringToControlMode(rclcpp::Node::SharedPtr nh, const std::string& control_modestr, ControlMode& control_mode);
+void speakError(rclcpp::Publisher<bitbots_msgs::msg::Audio>::SharedPtr speak_pub, const std::string& text);
 
 uint16_t dxlMakeword(uint64_t a, uint64_t b);
 uint32_t dxlMakedword(uint64_t a, uint64_t b);
-float dxlMakeFloat(uint8_t *data);
+float dxlMakeFloat(uint8_t* data);
 
 std::string gyroRangeToString(uint8_t range);
 std::string accelRangeToString(uint8_t range);

bitbots_ros_control/include/bitbots_ros_control/wolfgang_hardware_interface.hpp

@@ -32,7 +32,7 @@ class WolfgangHardwareInterface {
   // two dimensional list of all hardware interfaces, sorted by port
   std::vector<std::vector<bitbots_ros_control::HardwareInterface *>> interfaces_;
   DynamixelServoHardwareInterface servo_interface_;
-  rclcpp::Publisher<humanoid_league_msgs::msg::Audio>::SharedPtr speak_pub_;
+  rclcpp::Publisher<bitbots_msgs::msg::Audio>::SharedPtr speak_pub_;
 
   // prevent unnecessary error when power is turned on
   bool first_ping_error_;

bitbots_ros_control/launch/ros_control_standalone.launch

@@ -6,17 +6,19 @@
     <arg name="only_pressure" default="false"/>
     <arg name="robot_type" default="wolfgang"/>
 
-  <group>
-      <include file="$(find-pkg-share bitbots_utils)/launch/base.launch" >
-          <arg name="sim" value="$(var sim)"/>
-          <arg name="robot_type" value="$(var robot_type)"/>
-      </include>
+    <include file="$(find-pkg-share bitbots_utils)/launch/parameter_blackboard.launch" >
+        <arg name="sim" value="$(var sim)"/>
+    </include>
+
+    <include file="$(find-pkg-share bitbots_robot_description)/launch/load_robot_description.launch" >
+        <arg name="sim" value="$(var sim)"/>
+        <arg name="robot_type" value="$(var robot_type)"/>
+    </include>
 
     <include file="$(find-pkg-share bitbots_ros_control)/launch/ros_control.launch">
         <arg name="sim" value="$(var sim)"/>
         <arg name="torqueless_mode" value="$(var torqueless_mode)"/>
         <arg name="only_imu" value="$(var only_imu)"/>
         <arg name="only_pressure" value="$(var only_pressure)"/>
     </include>
-  </group>
 </launch>

bitbots_ros_control/package.xml

@@ -21,15 +21,16 @@
   <depend>bitbots_buttons</depend>
   <depend>bitbots_docs</depend>
   <depend>bitbots_msgs</depend>
-  <depend>dynamixel_workbench_toolbox</depend>
+  <depend>bitbots_robot_description</depend>
+  <depend>bitbots_utils</depend>
   <depend>controller_interface</depend>
   <depend>controller_manager</depend>
+  <depend>dynamixel_workbench_toolbox</depend>
   <depend>hardware_interface</depend>
-  <depend>humanoid_league_msgs</depend>
   <depend>humanoid_league_speaker</depend>
   <depend>pluginlib</depend>
-  <depend>realtime_tools</depend>
   <depend>rclcpp</depend>
+  <depend>realtime_tools</depend>
   <depend>std_msgs</depend>
   <depend>system_monitor</depend>
   <depend>transmission_interface</depend>

bitbots_ros_control/scripts/battery_led.py

@@ -9,29 +9,10 @@
 
 pub = node.create_publisher(ColorRGBA, "/led2", 1)
 
-led_full = ColorRGBA()
-led_full.a = 1.0
-led_full.r = 0.0
-led_full.g = 0.0
-led_full.b = 1.0
-
-led_mid = ColorRGBA()
-led_mid.a = 1.0
-led_mid.r = 0.0
-led_mid.g = 1.0
-led_mid.b = 0.0
-
-led_low = ColorRGBA()
-led_low.a = 1.0
-led_low.r = 1.0
-led_low.g = 0.0
-led_low.b = 0.0
-
-led_no = ColorRGBA()
-led_no.a = 1.0
-led_no.r = 0.0
-led_no.g = 0.0
-led_no.b = 0.0
+led_full = ColorRGBA(a=1.0, r=0.0, g=0.0, b=1.0)
+led_mid = ColorRGBA(a=1.0, r=0.0, g=1.0, b=0.0)
+led_low = ColorRGBA(a=1.0, r=1.0, g=0.0, b=0.0)
+led_no = ColorRGBA(a=1.0, r=0.0, g=0.0, b=0.0)
 
 
 def vbat_cb(msg: Float64):