Merge pull request #27 from westonrobot/feature-joy_msg

Feature joy msg
westonrobot · Jul 1, 2024 · 2dcabee · 2dcabee
2 parents 16a6876 + 3ab30c3
commit 2dcabee
Show file tree

Hide file tree

Showing 5 changed files with 7 additions and 23 deletions.
diff --git a/wrp_sdk_msgs/CMakeLists.txt b/wrp_sdk_msgs/CMakeLists.txt
@@ -26,7 +26,6 @@ set(message_files
   "msg/mobile_base/MotorState.msg"
   "msg/mobile_base/RangeData.msg"
   "msg/mobile_base/RangeDataArray.msg"
-  "msg/mobile_base/RcState.msg"
   "msg/mobile_base/SystemState.msg"
 
   # peripheral

diff --git a/wrp_sdk_msgs/msg/mobile_base/RcState.msg b/wrp_sdk_msgs/msg/mobile_base/RcState.msg
diff --git a/wrp_sdk_robot/README.md b/wrp_sdk_robot/README.md
@@ -56,7 +56,7 @@ Legend: Y - Supported, N - Not Supported, P - Partially Supported
 | `~/actuator_state` | wrp_sdk_msgs::msg::ActuatorStateArray | Outputs robot's actuator states          |
 | `~/odom`           | nav_msgs::msg::Odometry               | Outputs robot's wheel odometry           |
 | `~/battery_state`  | sensor_msgs::msg::BatteryState        | Outputs robot's battery state            |
-| `~/rc_state`       | wrp_sdk_msgs::msg::RcState            | Outputs robot's rc state (if applicable) |
+| `~/rc_state`       | sensor_msgs::msg::Joy            | Outputs robot's rc state (if applicable) |
 
 | Subscribed Topic | Type                      | Description              |
 | ---------------- | ------------------------- | ------------------------ |

diff --git a/wrp_sdk_robot/include/wrp_sdk_robot/mobile_base_node.hpp b/wrp_sdk_robot/include/wrp_sdk_robot/mobile_base_node.hpp
@@ -21,14 +21,12 @@
 #include <geometry_msgs/msg/quaternion.hpp>
 #include <nav_msgs/msg/odometry.hpp>
 #include <sensor_msgs/msg/battery_state.hpp>
+#include <sensor_msgs/msg/joy.hpp>
 #include "wrp_sdk_msgs/msg/system_state.hpp"
 #include "wrp_sdk_msgs/msg/motion_state.hpp"
 #include "wrp_sdk_msgs/msg/actuator_state_array.hpp"
 #include "wrp_sdk_msgs/msg/actuator_state.hpp"
-#include "wrp_sdk_msgs/msg/range_data.hpp"
-#include "wrp_sdk_msgs/msg/range_data_array.hpp"
 #include "wrp_sdk_msgs/msg/motion_command.hpp"
-#include "wrp_sdk_msgs/msg/rc_state.hpp"
 
 #include "wrp_sdk_msgs/srv/access_control.hpp"
 #include "wrp_sdk_msgs/srv/assisted_mode_control.hpp"
@@ -94,13 +92,9 @@ class MobileBaseNode : public rclcpp::Node {
       actuator_state_publisher_;
   rclcpp::Publisher<sensor_msgs::msg::BatteryState>::SharedPtr
       battery_state_publisher_;
-  rclcpp::Publisher<wrp_sdk_msgs::msg::RcState>::SharedPtr rc_state_publisher_;
+  rclcpp::Publisher<sensor_msgs::msg::Joy>::SharedPtr rc_state_publisher_;
 
   rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr odom_publisher_;
-  rclcpp::Publisher<wrp_sdk_msgs::msg::RangeDataArray>::SharedPtr
-      tof_data_publisher_;
-  rclcpp::Publisher<wrp_sdk_msgs::msg::RangeDataArray>::SharedPtr
-      ultrasonic_data_publisher_;
 
   rclcpp::Service<wrp_sdk_msgs::srv::AccessControl>::SharedPtr
       access_control_service_;

diff --git a/wrp_sdk_robot/src/mobile_base_node.cpp b/wrp_sdk_robot/src/mobile_base_node.cpp
@@ -145,20 +145,14 @@ bool MobileBaseNode::SetupInterfaces() {
       this->create_publisher<sensor_msgs::msg::BatteryState>("~/battery_state",
                                                              10);
   rc_state_publisher_ =
-      this->create_publisher<wrp_sdk_msgs::msg::RcState>("~/rc_state", 10);
+      this->create_publisher<sensor_msgs::msg::Joy>("~/rc_state", 10);
   odom_publisher_ =
       this->create_publisher<nav_msgs::msg::Odometry>("~/odom", 50);
 
   if (publish_odom_tf_) {
     // setup tf broadcaster
     tf_broadcaster_ = std::make_shared<tf2_ros::TransformBroadcaster>(this);
   }
-  ultrasonic_data_publisher_ =
-      this->create_publisher<wrp_sdk_msgs::msg::RangeDataArray>(
-          "~/ultrasonic_data", 10);
-  tof_data_publisher_ =
-      this->create_publisher<wrp_sdk_msgs::msg::RangeDataArray>("~/tof_data",
-                                                                10);
 
   // setup services
   access_control_service_ =
@@ -375,16 +369,16 @@ void MobileBaseNode::PublishActuatorState() {
 void MobileBaseNode::PublishRcState() {
   auto rc_state = robot_->GetRcState();
 
-  wrp_sdk_msgs::msg::RcState rc_state_msg;
+  sensor_msgs::msg::Joy rc_state_msg;
   rc_state_msg.header.stamp = this->now();
   rc_state_msg.header.frame_id = base_frame_;
 
   for (size_t i = 0; i < 8; i++) {
-    rc_state_msg.axes[i] = rc_state.axes[i];
+    rc_state_msg.axes.push_back(rc_state.axes[i]);
   }
 
   for (size_t i = 0; i < 8; i++) {
-    rc_state_msg.buttons[i] = rc_state.buttons[i];
+    rc_state_msg.buttons.push_back(rc_state.buttons[i]);
   }
 
   rc_state_publisher_->publish(rc_state_msg);