EmbodiedAI
/
z1_controller
mirror of https://github.com/unitreerobotics/z1_controller.git
3
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Merge 18394cba53 into 8eb6595658

This commit is contained in:
Paul Blum 2024-03-31 21:23:40 -04:00 committed by GitHub
parent 8eb6595658 18394cba53
commit 2c24ab5312
No known key found for this signature in database
GPG Key ID: B5690EEEBB952194
2 changed files with 24 additions and 18 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

39
sim/IOROS.cpp
View File

@ -8,16 +8,21 @@ void RosShutDown(int sig){
ros::shutdown();
}
IOROS::IOROS(){
IOROS::IOROS() : _rname("z1") {
std::cout << "The control interface for ROS Gazebo simulation" << std::endl;
hasGripper = false;
/* retrieve robot_name from ROS parameter server */
ros::param::get("/robot_name", _rname); // defaults to "z1" if parameter not found
std::cout << "robot_name: " << _rname << std::endl;
/* start subscriber */
_initRecv();
ros::AsyncSpinner subSpinner(1); // one threads
subSpinner.start();
usleep(300000); //wait for subscribers start
/* initialize publisher */
_initSend();
_initSend();
signal(SIGINT, RosShutDown);
@ -65,23 +70,23 @@ void IOROS::_recvState(LowlevelState *state){
}
void IOROS::_initSend(){
_servo_pub[0] = _nm.advertise<unitree_legged_msgs::MotorCmd>("/z1_gazebo/Joint01_controller/command", 1);
_servo_pub[1] = _nm.advertise<unitree_legged_msgs::MotorCmd>("/z1_gazebo/Joint02_controller/command", 1);
_servo_pub[2] = _nm.advertise<unitree_legged_msgs::MotorCmd>("/z1_gazebo/Joint03_controller/command", 1);
_servo_pub[3] = _nm.advertise<unitree_legged_msgs::MotorCmd>("/z1_gazebo/Joint04_controller/command", 1);
_servo_pub[4] = _nm.advertise<unitree_legged_msgs::MotorCmd>("/z1_gazebo/Joint05_controller/command", 1);
_servo_pub[5] = _nm.advertise<unitree_legged_msgs::MotorCmd>("/z1_gazebo/Joint06_controller/command", 1);
_servo_pub[6] = _nm.advertise<unitree_legged_msgs::MotorCmd>("/z1_gazebo/gripper_controller/command", 1);
_servo_pub[0] = _nm.advertise<unitree_legged_msgs::MotorCmd>("/" + _rname + "_gazebo/Joint01_controller/command", 1);
_servo_pub[1] = _nm.advertise<unitree_legged_msgs::MotorCmd>("/" + _rname + "_gazebo/Joint02_controller/command", 1);
_servo_pub[2] = _nm.advertise<unitree_legged_msgs::MotorCmd>("/" + _rname + "_gazebo/Joint03_controller/command", 1);
_servo_pub[3] = _nm.advertise<unitree_legged_msgs::MotorCmd>("/" + _rname + "_gazebo/Joint04_controller/command", 1);
_servo_pub[4] = _nm.advertise<unitree_legged_msgs::MotorCmd>("/" + _rname + "_gazebo/Joint05_controller/command", 1);
_servo_pub[5] = _nm.advertise<unitree_legged_msgs::MotorCmd>("/" + _rname + "_gazebo/Joint06_controller/command", 1);
_servo_pub[6] = _nm.advertise<unitree_legged_msgs::MotorCmd>("/" + _rname + "_gazebo/gripper_controller/command", 1);
}
void IOROS::_initRecv(){
_servo_sub[0] = _nm.subscribe("/z1_gazebo/Joint01_controller/state", 1, &IOROS::_joint00Callback, this);
_servo_sub[1] = _nm.subscribe("/z1_gazebo/Joint02_controller/state", 1, &IOROS::_joint01Callback, this);
_servo_sub[2] = _nm.subscribe("/z1_gazebo/Joint03_controller/state", 1, &IOROS::_joint02Callback, this);
_servo_sub[3] = _nm.subscribe("/z1_gazebo/Joint04_controller/state", 1, &IOROS::_joint03Callback, this);
_servo_sub[4] = _nm.subscribe("/z1_gazebo/Joint05_controller/state", 1, &IOROS::_joint04Callback, this);
_servo_sub[5] = _nm.subscribe("/z1_gazebo/Joint06_controller/state", 1, &IOROS::_joint05Callback, this);
_servo_sub[6] = _nm.subscribe("/z1_gazebo/gripper_controller/state", 1, &IOROS::_gripperCallback, this);
_servo_sub[0] = _nm.subscribe("/" + _rname + "_gazebo/Joint01_controller/state", 1, &IOROS::_joint00Callback, this);
_servo_sub[1] = _nm.subscribe("/" + _rname + "_gazebo/Joint02_controller/state", 1, &IOROS::_joint01Callback, this);
_servo_sub[2] = _nm.subscribe("/" + _rname + "_gazebo/Joint03_controller/state", 1, &IOROS::_joint02Callback, this);
_servo_sub[3] = _nm.subscribe("/" + _rname + "_gazebo/Joint04_controller/state", 1, &IOROS::_joint03Callback, this);
_servo_sub[4] = _nm.subscribe("/" + _rname + "_gazebo/Joint05_controller/state", 1, &IOROS::_joint04Callback, this);
_servo_sub[5] = _nm.subscribe("/" + _rname + "_gazebo/Joint06_controller/state", 1, &IOROS::_joint05Callback, this);
_servo_sub[6] = _nm.subscribe("/" + _rname + "_gazebo/gripper_controller/state", 1, &IOROS::_gripperCallback, this);
}
void IOROS::_joint00Callback(const unitree_legged_msgs::MotorState& msg){
@ -139,4 +144,4 @@ void IOROS::_gripperCallback(const unitree_legged_msgs::MotorState& msg){
_joint_state[6].dq = msg.dq;
_joint_state[6].ddq = msg.ddq;
_joint_state[6].tauEst = msg.tauEst;
}
}

3
sim/IOROS.h
View File

@ -13,6 +13,7 @@ public:
bool sendRecv(const LowlevelCmd *cmd, LowlevelState *state);
private:
ros::NodeHandle _nm;
std::string _rname; // robot_name retrieved from ROS parameter server
ros::Subscriber _servo_sub[7];
ros::Publisher _servo_pub[7];
unitree_legged_msgs::MotorState _joint_state[7];
@ -31,4 +32,4 @@ private:
void _gripperCallback(const unitree_legged_msgs::MotorState& msg);
};
#endif // IOROS_H
#endif // IOROS_H