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

basic lowlevel bridge functionality added.

This commit is contained in:
Rooholla-Khorrambakht 2024-01-04 14:12:41 -05:00
parent cea501e2cb
commit 95cff05d6a
3 changed files with 80 additions and 86 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
Go2Py/simple_lowcmd_sender.py
View File

@ -24,4 +24,4 @@ cmd = LowCmd(
)
while True:
dw.write(cmd)
time.sleep(0.1)
time.sleep(0.01)

1
cpp_bridge/include/utils/dds_subscriber.hpp
View File

@ -13,6 +13,7 @@ template<typename T>
class DDSSubscriber {
public:
// Define a callback type for user-provided callbacks
using UserCallback = std::function<void(const T&)>;
private:

163
cpp_bridge/src/lowlevel/bridge.cpp
View File

@ -11,6 +11,9 @@
#include <go2py/LowCmd.hpp>
#include <go2py/LowState.hpp>
#include <thread>
#include "utils/dds_subscriber.hpp"
#include "utils/dds_publisher.hpp"
using namespace unitree::common;
using namespace unitree::robot;
@ -20,44 +23,6 @@ using namespace org::eclipse::cyclonedds;
constexpr double PosStopF = (2.146E+9f);
constexpr double VelStopF = (16000.0f);
class Bridge
{
public:
explicit Bridge()
{
}
~Bridge()
{
}
// participant = dds::domain::DomainParticipant(domain_id);
void Init();
private:
void InitLowCmd();
void LowStateMessageHandler(const void* messages);
void LowCmdWrite();
std::thread go2py_thread;
bool running = true;
void go2py_callback();
// Go2PyListener go2py_listener;
// dds::sub::qos::DataReaderQos rqos;
private:
float dt = 0.002; // 0.001~0.01
unitree_go::msg::dds_::LowCmd_ low_cmd{}; // default init
unitree_go::msg::dds_::LowState_ low_state{}; // default init
/*publisher*/
ChannelPublisherPtr<unitree_go::msg::dds_::LowCmd_> lowcmd_publisher;
/*subscriber*/
ChannelSubscriberPtr<unitree_go::msg::dds_::LowState_> lowstate_subscriber;
/*LowCmd write thread*/
ThreadPtr lowCmdWriteThreadPtr;
// dds::topic::Topic<unitree_go::msg::dds_::LowCmd_> topic(participant, "go2py/lowcmd");
};
uint32_t crc32_core(uint32_t* ptr, uint32_t len)
{
unsigned int xbit = 0;
@ -90,7 +55,47 @@ uint32_t crc32_core(uint32_t* ptr, uint32_t len)
return CRC32;
}
void Bridge::Init()
class Go2LowLevelBridge
{
public:
explicit Go2LowLevelBridge(const std::string &input_cmd_topic_name,
const std::string &output_state_topic_name,
const int domain_id = 0):
lowstate_publisher(output_state_topic_name),
lowcmd_subscriber(input_cmd_topic_name, std::bind(&Go2LowLevelBridge::go2py_callback, this, std::placeholders::_1))
{
ChannelFactory::Instance()->Init(domain_id);
}
~Go2LowLevelBridge()
{
}
void Init();
private:
void InitLowCmd();
void LowStateMessageHandler(const void* messages);
void LowCmdWrite();
std::thread go2py_thread;
bool running = true;
void go2py_callback(const msgs::LowCmd& msg);
DDSPublisher<msgs::LowState> lowstate_publisher;
DDSSubscriber<msgs::LowCmd> lowcmd_subscriber;
private:
float dt = 0.002; // 0.001~0.01
unitree_go::msg::dds_::LowCmd_ low_cmd{}; // default init
unitree_go::msg::dds_::LowState_ low_state{}; // default init
/*publisher*/
ChannelPublisherPtr<unitree_go::msg::dds_::LowCmd_> lowcmd_publisher;
/*subscriber*/
ChannelSubscriberPtr<unitree_go::msg::dds_::LowState_> lowstate_subscriber;
/*LowCmd write thread*/
ThreadPtr lowCmdWriteThreadPtr;
};
void Go2LowLevelBridge::Init()
{
InitLowCmd();
/*create publisher*/
@ -98,56 +103,26 @@ void Bridge::Init()
lowcmd_publisher->InitChannel();
/*create subscriber*/
lowstate_subscriber.reset(new ChannelSubscriber<unitree_go::msg::dds_::LowState_>(TOPIC_LOWSTATE));
lowstate_subscriber->InitChannel(std::bind(&Bridge::LowStateMessageHandler, this, std::placeholders::_1), 1);
lowstate_subscriber->InitChannel(std::bind(&Go2LowLevelBridge::LowStateMessageHandler, this, std::placeholders::_1), 1);
/*loop publishing thread*/
lowCmdWriteThreadPtr = CreateRecurrentThreadEx("writebasiccmd", UT_CPU_ID_NONE, 2000, &Bridge::LowCmdWrite, this);
go2py_thread = std::thread(&Bridge::go2py_callback, this);
// lowCmdWriteThreadPtr = CreateRecurrentThreadEx("writebasiccmd", UT_CPU_ID_NONE, 2000, &Go2LowLevelBridge::LowCmdWrite, this);
}
void Bridge::go2py_callback()
void Go2LowLevelBridge::go2py_callback(const msgs::LowCmd& msg)
{
// dds::domain::DomainParticipant participant(domain::default_id());
// /* To subscribe to something, a topic is needed. */
// dds::topic::Topic<msgs::LowCmd> topic(participant, "go2py/lowcmd");
// /* A reader also needs a subscriber. */
// // dds::sub::Subscriber subscriber(participant);
// dds::sub::NoOpSubscriberListener qlistener; /*you need to create your own class that derives from this listener, and implement your own callbacks*/
// /*the listener implementation should implement the on_subscription_matched virtual function as we will rely on it later*/
// dds::sub::qos::SubscriberQos subqos; /*add custom QoS policies that you want for this subscriber*/
// dds::sub::Subscriber sub(participant, subqos, &qlistener, dds::core::status::StatusMask::subscription_matched());
// /* Now, the reader can be created to subscribe to a HelloWorld message. */
// dds::sub::NoOpAnyDataReaderListener listener; /*you need to create your own class that derives from this listener, and implement your own callback functions*/
// /*the listener implementation should implement the on_data_available virtual function as we will rely on it later*/
// dds::sub::qos::DataReaderQos rqos;
// // dds::sub::DataReader<msgs::LowCmd> reader(subscriber, topic, rqos, &listener, dds::core::status::StatusMask::data_available());
// dds::sub::DataReader<msgs::LowCmd> reader(sub, topic, rqos, &listener, dds::core::status::StatusMask::data_available());
// dds::sub::LoanedSamples<msgs::LowCmd> samples;
while (running)
for(int i=0; i<12; i++)
{
// /* Try taking samples from the reader. */
// samples = reader.take();
// /* Are samples read? */
// if (samples.length() > 0) {
// /* Use an iterator to run over the set of samples. */
// dds::sub::LoanedSamples<msgs::LowCmd>::const_iterator sample_iter;
// for (sample_iter = samples.begin();
// sample_iter < samples.end();
// ++sample_iter) {
// /* Get the message and sample information. */
// const msgs::LowCmd& msg = sample_iter->data();
// const dds::sub::SampleInfo& info = sample_iter->info();
// std::cout << "go2py_callback got a sample..." << std::endl;
// }
// }
std::this_thread::sleep_for(std::chrono::seconds(1));
low_cmd.motor_cmd()[i].q() = msg.q()[i];
low_cmd.motor_cmd()[i].dq() = msg.dq()[i];
low_cmd.motor_cmd()[i].kp() = msg.kp()[i];
low_cmd.motor_cmd()[i].kd() = msg.kv()[i];
low_cmd.motor_cmd()[i].tau() = msg.tau_ff()[i];
}
std::cout << "go2py_callback stopped." << std::endl;
low_cmd.crc() = crc32_core((uint32_t *)&low_cmd, (sizeof(unitree_go::msg::dds_::LowCmd_)>>2)-1);
lowcmd_publisher->Write(low_cmd);
}
void Bridge::InitLowCmd()
void Go2LowLevelBridge::InitLowCmd()
{
low_cmd.head()[0] = 0xFE;
low_cmd.head()[1] = 0xEF;
@ -165,12 +140,30 @@ void Bridge::InitLowCmd()
}
}
void Bridge::LowStateMessageHandler(const void* message)
void Go2LowLevelBridge::LowStateMessageHandler(const void* message)
{
low_state = *(unitree_go::msg::dds_::LowState_*)message;
msgs::LowState lowstate;
for(int i=0; i<12; i++)
{
lowstate.q()[i] = low_state.motor_state()[i].q();
lowstate.dq()[i] = low_state.motor_state()[i].dq();
lowstate.ddq()[i] = low_state.motor_state()[i].ddq();
lowstate.tau_est()[i] = low_state.motor_state()[i].tau_est();
lowstate.tmp()[i] = (float)low_state.motor_state()[i].temperature();
}
lowstate.voltage() = (float)low_state.power_v();
lowstate.current() = (float)low_state.power_a();
for(int i=0; i<4; i++)
lowstate.contact()[i] = (float)low_state.foot_force()[i];
lowstate_publisher.publish(lowstate);
}
void Bridge::LowCmdWrite()
void Go2LowLevelBridge::LowCmdWrite()
{
low_cmd.motor_cmd()[2].q() = 0;
low_cmd.motor_cmd()[2].dq() = 0;
@ -189,8 +182,8 @@ int main(int argc, const char** argv)
exit(-1);
}
ChannelFactory::Instance()->Init(0, argv[1]);
Bridge bridge;
// ChannelFactory::Instance()->Init(0, argv[1]);
Go2LowLevelBridge bridge("go2py/lowcmd", "go2py/lowstate");
bridge.Init();
while (1)
{