Skip to content

Instantly share code, notes, and snippets.

@include-yy
Created October 19, 2023 06:20
Show Gist options
  • Save include-yy/4fdc142eb959e783eadce05a9c310371 to your computer and use it in GitHub Desktop.
Save include-yy/4fdc142eb959e783eadce05a9c310371 to your computer and use it in GitHub Desktop.
GO1

https://ww2.mathworks.cn/help/ros/ug/get-started-with-ros.html

https://jp.mathworks.com/help/ros/ref/send.html

cmake_minimum_required(VERSION 3.14)
project(unitree_guide)
set(ROBOT_TYPE Go1) # The type of robot, support Go1 and A1 currently
set(PLATFORM amd64) # The platform to compile, support amd64 and arm64
set(CATKIN_MAKE ON) # Use CATKIN_MAKE or not, ON or OFF
set(SIMULATION ON) # Use Gazebo or not, ON or OFF
set(REAL_ROBOT OFF) # Link real robot or not, ON or OFF
set(DEBUG OFF) # Use debug functions or not, ON or OFF
set(MOVE_BASE OFF) # Need move_base or not, ON or OFF
if(NOT DEFINED ROBOT_TYPE)
message(FATAL_ERROR "[CMake ERROR] Have not defined ROBOT_TYPE")
endif()
if(NOT DEFINED PLATFORM)
message(FATAL_ERROR "[CMake ERROR] Have not defined PLATFORM")
endif()
if(${ROBOT_TYPE} STREQUAL "A1")
add_definitions(-DROBOT_TYPE_A1)
elseif(${ROBOT_TYPE} STREQUAL "Go1")
add_definitions(-DROBOT_TYPE_Go1)
else()
message(FATAL_ERROR "[CMake ERROR] The ROBOT_TYPE is error")
endif()
if(((SIMULATION) AND (REAL_ROBOT)) OR ((NOT SIMULATION) AND (NOT REAL_ROBOT)))
message(FATAL_ERROR "[CMake ERROR] The SIMULATION and REAL_ROBOT can only be one ON one OFF")
endif()
if(SIMULATION OR MOVE_BASE)
add_definitions(-DRUN_ROS)
set(CATKIN_MAKE ON)
endif()
set(CMAKE_CXX_STANDARD 11)
find_package(Boost)
if(CATKIN_MAKE)
add_definitions(-DCOMPILE_WITH_ROS)
if(MOVE_BASE)
add_definitions(-DCOMPILE_WITH_MOVE_BASE)
endif()
endif()
if(DEBUG)
add_definitions(-DCOMPILE_DEBUG)
find_package(Python2 COMPONENTS Interpreter Development NumPy)
endif()
if(CATKIN_MAKE)
if(SIMULATION)
add_definitions(-DCOMPILE_WITH_SIMULATION)
find_package(catkin REQUIRED COMPONENTS
controller_manager
joint_state_controller
gazebo_ros
# gazebo
)
endif()
find_package(catkin REQUIRED COMPONENTS
genmsg
robot_state_publisher
roscpp
std_msgs
tf
geometry_msgs
unitree_legged_msgs
)
catkin_package(
CATKIN_DEPENDS
unitree_legged_msgs
)
endif()
include_directories(
include
${Boost_INCLUDE_DIRS}
${catkin_INCLUDE_DIRS}
${GAZEBO_INCLUDE_DIRS}
)
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${GAZEBO_CXX_FLAGS} -O3")
if(REAL_ROBOT)
add_definitions(-DCOMPILE_WITH_REAL_ROBOT)
if(${ROBOT_TYPE} STREQUAL "A1")
include_directories(
library/unitree_legged_sdk_3.2/include
)
link_directories(
library/unitree_legged_sdk_3.2/lib
)
elseif(${ROBOT_TYPE} STREQUAL "Go1")
include_directories(
library/unitree_legged_sdk-3.8.0/include
)
if(${PLATFORM} STREQUAL "amd64")
link_directories(
library/unitree_legged_sdk-3.8.0/lib/cpp/amd64
)
elseif(${PLATFORM} STREQUAL "arm64")
link_directories(
library/unitree_legged_sdk-3.8.0/lib/cpp/arm64
)
endif()
endif()
endif()
link_directories(
${GAZEBO_LIBRARY_DIRS}
)
# aux_source_directory(src SRC_LIST)
file(GLOB_RECURSE SRC_LIST
"src/*/*.cpp"
"src/*/*.cc"
)
# add here @1
add_executable(yy_ctrl src/yy.cpp ${SRC_LIST})
add_executable(junior_ctrl src/main.cpp ${SRC_LIST})
if(CATKIN_MAKE)
target_link_libraries(junior_ctrl ${catkin_LIBRARIES})
# add here @2
target_link_libraries(yy_ctrl ${catkin_LIBRARIES})
add_dependencies(junior_ctrl ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
# add here @3
add_dependencies(yy_ctrl ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
endif()
if(DEBUG)
target_include_directories(junior_ctrl PRIVATE ${Python2_INCLUDE_DIRS} ${Python2_NumPy_INCLUDE_DIRS})
target_link_libraries(junior_ctrl Python2::Python Python2::NumPy)
endif()
if(REAL_ROBOT)
if(${ROBOT_TYPE} STREQUAL "A1")
if(${PLATFORM} STREQUAL "amd64")
target_link_libraries(junior_ctrl libunitree_legged_sdk_amd64.so)
elseif(${PLATFORM} STREQUAL "arm64")
target_link_libraries(junior_ctrl libunitree_legged_sdk_arm64.so)
endif()
elseif(${ROBOT_TYPE} STREQUAL "Go1")
target_link_libraries(junior_ctrl libunitree_legged_sdk.a)
endif()
endif()
target_link_libraries(junior_ctrl -pthread lcm)
if(NOT CATKIN_MAKE)
set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin)
endif()
/**********************************************************************
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;
}
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment