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

switch to lowlevel mode key added to the bridge

This commit is contained in:
Rooholla-KhorramBakht 2024-04-16 00:05:02 -04:00
parent a43e0bbc16
commit 94d5d652d7
2 changed files with 50 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -24,7 +24,7 @@
#include "unitree_api/msg/request.hpp"
#include "common/ros2_sport_client.h"
#include "joystick.h"
#include "unitree_api/msg/request.hpp"
class Custom: public rclcpp::Node
{
public:
@ -53,6 +53,7 @@ class Custom: public rclcpp::Node
"/go2/lowcmd", 1, std::bind(&Custom::lowcmd_callback, this, std::placeholders::_1));
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);
}
private:
@ -89,6 +90,7 @@ class Custom: public rclcpp::Node
unitree_go::msg::LowCmd lowcmd_msg;
uint64_t last_lowcmd_stamp = 0;
xRockerBtnDataStruct _keyData;
rclcpp::Publisher<unitree_api::msg::Request>::SharedPtr api_publisher;
void init_lowcmd()
{
@ -190,11 +192,28 @@ void Custom::lowstate_callback(unitree_go::msg::LowState::SharedPtr data)
{
Estop = 1;
}
if ((_keyData.btn.components.R1 == 1 && _keyData.btn.components.L1 == 1)||
(_keyData.btn.components.L2 == 1 && _keyData.btn.components.A == 1))
if ((_keyData.btn.components.L2 == 1 && _keyData.btn.components.A == 1))
{
Estop = 0;
}
if ((_keyData.btn.components.L2 == 1 && _keyData.btn.components.L1 == 1))
{
auto msg = std::make_shared<unitree_api::msg::Request>();
// Populate the message fields
msg->header.identity.id = 80005;
msg->header.identity.api_id = 1001;
msg->parameter = "{\"name\":\"sport_mode\",\"switch\":0}";
api_publisher->publish(*msg);
}
if ((_keyData.btn.components.R2 == 1 && _keyData.btn.components.R1 == 1))
{
auto msg = std::make_shared<unitree_api::msg::Request>();
// Populate the message fields
msg->header.identity.id = 80005;
msg->header.identity.api_id = 1001;
msg->parameter = "{\"name\":\"sport_mode\",\"switch\":1}";
api_publisher->publish(*msg);
}
// std::cout << "Estop: " << Estop << std::endl;
}

28
req.py Normal file
View File

@ -0,0 +1,28 @@
import rclpy
from rclpy.node import Node
from unitree_api.msg import Request
class PublisherNode(Node):
def __init__(self):
super().__init__('publisher_node')
self.publisher_ = self.create_publisher(Request, '/api/robot_state/request', 10)
self.timer = self.create_timer(1, self.publish_message)
def publish_message(self):
msg = Request()
# Populate the message fields
msg.header.identity.id=80005
msg.header.identity.api_id=1001
msg.parameter = '{"name":"sport_mode","switch":0}'
self.publisher_.publish(msg)
self.get_logger().info('Published message')
def main(args=None):
rclpy.init(args=args)
node = PublisherNode()
rclpy.spin(node)
node.destroy_node()
rclpy.shutdown()
if __name__ == '__main__':
main()