robot: use joy msg instead of rc state

westonrobot · Jul 1, 2024 · 04bacac · 04bacac
1 parent 2183db5
commit 04bacac
Show file tree

Hide file tree

Showing 3 changed files with 7 additions and 9 deletions.
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/include/wrp_sdk_robot/mobile_base_node.hpp b/wrp_sdk_robot/include/wrp_sdk_robot/mobile_base_node.hpp
@@ -21,6 +21,7 @@
 #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"
@@ -92,7 +93,7 @@ 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_;
 

diff --git a/wrp_sdk_robot/src/mobile_base_node.cpp b/wrp_sdk_robot/src/mobile_base_node.cpp
@@ -145,15 +145,15 @@ 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);
   }
-  
+
   // setup services
   access_control_service_ =
       this->create_service<wrp_sdk_msgs::srv::AccessControl>(
@@ -369,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);