EmbodiedAI
/
Go2Py_SIM
mirror of https://github.com/machines-in-motion/Go2Py.git
3
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

go2py status publisher is added to the bridge

This commit is contained in:
Rooholla-KhorramBakht 2024-04-16 00:29:05 -04:00
parent 7d0bf5f6f4
commit df09337192
3 changed files with 13 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
deploy/robot_ws/src/go2py_node/src/bridge.cpp
View File

@ -1,6 +1,7 @@
#include "unitree_go/msg/low_state.hpp" #include "unitree_go/msg/low_state.hpp"
#include "unitree_go/msg/imu_state.hpp" #include "unitree_go/msg/imu_state.hpp"
#include "unitree_go/msg/motor_state.hpp" #include "unitree_go/msg/motor_state.hpp"
#include "unitree_go/msg/go2py_status.hpp"
#include "unitree_go/msg/go2py_low_cmd.hpp" #include "unitree_go/msg/go2py_low_cmd.hpp"
#include "unitree_go/msg/low_cmd.hpp" #include "unitree_go/msg/low_cmd.hpp"
@ -54,6 +55,7 @@ class Custom: public rclcpp::Node
lowcmd_puber = this->create_publisher<unitree_go::msg::LowCmd>("/lowcmd", 10); lowcmd_puber = this->create_publisher<unitree_go::msg::LowCmd>("/lowcmd", 10);
api_publisher = this->create_publisher<unitree_api::msg::Request>("/api/robot_state/request", 10); api_publisher = this->create_publisher<unitree_api::msg::Request>("/api/robot_state/request", 10);
status_publisher = this->create_publisher<unitree_go::msg::Go2pyStatus>("/go2py/status", 10);
} }
private: private:
@ -91,6 +93,7 @@ class Custom: public rclcpp::Node
uint64_t last_lowcmd_stamp = 0; uint64_t last_lowcmd_stamp = 0;
xRockerBtnDataStruct _keyData; xRockerBtnDataStruct _keyData;
rclcpp::Publisher<unitree_api::msg::Request>::SharedPtr api_publisher; rclcpp::Publisher<unitree_api::msg::Request>::SharedPtr api_publisher;
rclcpp::Publisher<unitree_go::msg::Go2pyStatus>::SharedPtr status_publisher;
void init_lowcmd() void init_lowcmd()
{ {
@ -125,6 +128,7 @@ class Custom: public rclcpp::Node
float foot_force[4]; float foot_force[4];
}highstate; }highstate;
int Estop = 0; int Estop = 0;
int sport_mode = 1;
}; };
void Custom::lowstate_callback(unitree_go::msg::LowState::SharedPtr data) void Custom::lowstate_callback(unitree_go::msg::LowState::SharedPtr data)
@ -204,6 +208,7 @@ void Custom::lowstate_callback(unitree_go::msg::LowState::SharedPtr data)
msg->header.identity.api_id = 1001; msg->header.identity.api_id = 1001;
msg->parameter = "{\"name\":\"sport_mode\",\"switch\":0}"; msg->parameter = "{\"name\":\"sport_mode\",\"switch\":0}";
api_publisher->publish(*msg); api_publisher->publish(*msg);
sport_mode = 0;
} }
if ((_keyData.btn.components.R2 == 1 && _keyData.btn.components.R1 == 1)) if ((_keyData.btn.components.R2 == 1 && _keyData.btn.components.R1 == 1))
{ {
@ -213,6 +218,7 @@ void Custom::lowstate_callback(unitree_go::msg::LowState::SharedPtr data)
msg->header.identity.api_id = 1001; msg->header.identity.api_id = 1001;
msg->parameter = "{\"name\":\"sport_mode\",\"switch\":1}"; msg->parameter = "{\"name\":\"sport_mode\",\"switch\":1}";
api_publisher->publish(*msg); api_publisher->publish(*msg);
sport_mode = 1;
} }
// std::cout << "Estop: " << Estop << std::endl; // std::cout << "Estop: " << Estop << std::endl;
} }
@ -340,6 +346,10 @@ void Custom::watchdog()
} }
lowcmd_puber->publish(lowcmd_msg); lowcmd_puber->publish(lowcmd_msg);
} }
auto msg = std::make_shared<unitree_go::msg::Go2pyStatus>();
msg->estop = Estop;
msg->sport_mode = sport_mode;
status_publisher->publish(*msg);
} }
int main(int argc, char **argv) int main(int argc, char **argv)

1
deploy/robot_ws/src/unitree/unitree_go/CMakeLists.txt
View File

@ -49,6 +49,7 @@ rosidl_generate_interfaces(${PROJECT_NAME}
"msg/UwbSwitch.msg" "msg/UwbSwitch.msg"
"msg/WirelessController.msg" "msg/WirelessController.msg"
"msg/Go2pyLowCmd.msg" "msg/Go2pyLowCmd.msg"
"msg/Go2pyStatus.msg"
DEPENDENCIES geometry_msgs DEPENDENCIES geometry_msgs
) )

2
deploy/robot_ws/src/unitree/unitree_go/msg/Go2pyStatus.msg Normal file
View File

@ -0,0 +1,2 @@
uint8 estop
uint8 sport_mode