|
/********************************************************************** |
|
Copyright (c) 2020-2023, Unitree Robotics.Co.Ltd. All rights reserved. |
|
***********************************************************************/ |
|
#include <iostream> |
|
#include <unistd.h> |
|
#include <csignal> |
|
#include <sched.h> |
|
|
|
#include "control/ControlFrame.h" |
|
#include "control/CtrlComponents.h" |
|
|
|
#include "Gait/WaveGenerator.h" |
|
|
|
#include "interface/KeyBoard.h" |
|
#include "interface/IOROS.h" |
|
|
|
#include <std_msgs/Int32.h> |
|
#include <geometry_msgs/Point.h> |
|
|
|
// new class inherited from IOROS |
|
|
|
class YYROS : public IOROS { |
|
public: |
|
YYROS(CmdPanel *myCmdPanel); |
|
~YYROS(); |
|
}; |
|
// use another control pannel instead of Keyboard |
|
YYROS::YYROS(CmdPanel *myCmdPanel):IOROS::IOROS() { |
|
delete cmdPanel; |
|
cmdPanel = myCmdPanel; |
|
} |
|
// do nothing |
|
YYROS::~YYROS() {} |
|
|
|
class YYPanel : public CmdPanel { |
|
public: |
|
YYPanel(); |
|
~YYPanel(); |
|
private: |
|
void* run (void *arg); |
|
static void* runyy(void *arg); |
|
pthread_t _tid; |
|
void checkCmdCallback(const std_msgs::Int32 i); |
|
void changeValueCallback(const geometry_msgs::Point p); |
|
|
|
// ros specified variable |
|
// state change listener; |
|
ros::Subscriber yycmd; |
|
// velocity change listener; |
|
ros::Subscriber yyvalue; |
|
}; |
|
|
|
YYPanel::YYPanel() { |
|
userCmd = UserCommand::NONE; |
|
userValue.setZero(); |
|
ros::NodeHandle n; |
|
// register message callback functions |
|
yycmd = n.subscribe("yycmd", 1, &YYPanel::checkCmdCallback, this); |
|
yyvalue = n.subscribe("yyvalue", 1, &YYPanel::changeValueCallback, this); |
|
pthread_create(&_tid, NULL, runyy, (void*)this); |
|
} |
|
|
|
YYPanel::~YYPanel() { |
|
pthread_cancel(_tid); |
|
pthread_join(_tid, NULL); |
|
} |
|
|
|
void* YYPanel::runyy(void *arg) { |
|
((YYPanel*)arg)->run(NULL); |
|
return NULL; |
|
} |
|
|
|
void* YYPanel::run(void *arg) { |
|
ros::MultiThreadedSpinner spinner(4); |
|
spinner.spin(); |
|
return NULL; |
|
} |
|
|
|
void YYPanel::checkCmdCallback(std_msgs::Int32 i) { |
|
ROS_INFO("%d", i.data); |
|
//* |
|
UserCommand tmp; |
|
switch (i.data){ |
|
case 1: |
|
tmp = UserCommand::L2_B; |
|
break; |
|
case 2: |
|
tmp = UserCommand::L2_A; |
|
break; |
|
case 3: |
|
tmp = UserCommand::L2_X; |
|
break; |
|
case 4: |
|
tmp = UserCommand::START; |
|
break; |
|
#ifdef COMPILE_WITH_MOVE_BASE |
|
case 5: |
|
tmp = UserCommand::L2_Y; |
|
break; |
|
#endif // COMPILE_WITH_MOVE_BASE |
|
case 6: |
|
tmp = UserCommand::L1_X; |
|
break; |
|
case 9: |
|
tmp = UserCommand::L1_A; |
|
break; |
|
case 8: |
|
tmp = UserCommand::L1_Y; |
|
break; |
|
case 0: |
|
userValue.setZero(); |
|
tmp = UserCommand::NONE; |
|
break; |
|
default: |
|
tmp = UserCommand::NONE; |
|
break; |
|
} |
|
userCmd = tmp; |
|
//*/ |
|
} |
|
|
|
void YYPanel::changeValueCallback(const geometry_msgs::Point p) |
|
{ |
|
//ROS_INFO("speed: %f, %f, %f", p.x, p.y, p.z); |
|
// (x, y, z) |
|
// x for x-axis speed, y for y-axis speed, z for rotate speed |
|
//* |
|
userValue.lx = p.x; |
|
userValue.ly = p.y; |
|
userValue.rx = p.z; |
|
//*/ |
|
} |
|
|
|
/* |
|
|
|
*/ |
|
|
|
bool running = true; |
|
|
|
// over watch the ctrl+c command |
|
void ShutDown(int sig) |
|
{ |
|
std::cout << "stop the controller" << std::endl; |
|
running = false; |
|
} |
|
|
|
int main(int argc, char **argv) |
|
{ |
|
/* set the print format */ |
|
std::cout << std::fixed << std::setprecision(3); |
|
|
|
ros::init(argc, argv, "unitree_gazebo_servo"); |
|
|
|
IOInterface *ioInter; |
|
CtrlPlatform ctrlPlat; |
|
|
|
ioInter = new YYROS(new YYPanel()); |
|
ctrlPlat = CtrlPlatform::GAZEBO; |
|
|
|
CtrlComponents *ctrlComp = new CtrlComponents(ioInter); |
|
ctrlComp->ctrlPlatform = ctrlPlat; |
|
ctrlComp->dt = 0.002; // run at 500hz |
|
ctrlComp->running = &running; |
|
|
|
ctrlComp->robotModel = new Go1Robot(); |
|
|
|
ctrlComp->waveGen = new WaveGenerator(0.45, 0.5, Vec4(0, 0.5, 0.5, 0)); // Trot |
|
// ctrlComp->waveGen = new WaveGenerator(1.1, 0.75, Vec4(0, 0.25, 0.5, 0.75)); //Crawl, only for sim |
|
//ctrlComp->waveGen = new WaveGenerator(0.4, 0.6, Vec4(0, 0.5, 0.5, 0)); //Walking Trot, only for sim |
|
//ctrlComp->waveGen = new WaveGenerator(0.4, 0.35, Vec4(0, 0.5, 0.5, 0)); //Running Trot, only for sim |
|
// ctrlComp->waveGen = new WaveGenerator(0.4, 0.7, Vec4(0, 0, 0, 0)); //Pronk, only for sim |
|
|
|
ctrlComp->geneObj(); |
|
|
|
ControlFrame ctrlFrame(ctrlComp); |
|
|
|
// deal with Ctrl+C |
|
signal(SIGINT, ShutDown); |
|
|
|
while (running) |
|
{ |
|
ctrlFrame.run(); |
|
} |
|
|
|
delete ctrlComp; |
|
return 0; |
|
} |