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

usage of robot_name parameter in IOROS; enables sim_ctrl of Z1 when mounted on quadruped

This commit is contained in:
Paul 2024-03-28 21:56:40 -04:00
parent 8eb6595658
commit 18394cba53
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(); ros::shutdown();
} }
IOROS::IOROS(){ IOROS::IOROS() : _rname("z1") {
std::cout << "The control interface for ROS Gazebo simulation" << std::endl; std::cout << "The control interface for ROS Gazebo simulation" << std::endl;
hasGripper = false; 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 */ /* start subscriber */
_initRecv(); _initRecv();
ros::AsyncSpinner subSpinner(1); // one threads ros::AsyncSpinner subSpinner(1); // one threads
subSpinner.start(); subSpinner.start();
usleep(300000); //wait for subscribers start usleep(300000); //wait for subscribers start
/* initialize publisher */ /* initialize publisher */
_initSend(); _initSend();
signal(SIGINT, RosShutDown); signal(SIGINT, RosShutDown);
@ -65,23 +70,23 @@ void IOROS::_recvState(LowlevelState *state){
} }
void IOROS::_initSend(){ void IOROS::_initSend(){
_servo_pub[0] = _nm.advertise<unitree_legged_msgs::MotorCmd>("/z1_gazebo/Joint01_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>("/z1_gazebo/Joint02_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>("/z1_gazebo/Joint03_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>("/z1_gazebo/Joint04_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>("/z1_gazebo/Joint05_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>("/z1_gazebo/Joint06_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>("/z1_gazebo/gripper_controller/command", 1); _servo_pub[6] = _nm.advertise<unitree_legged_msgs::MotorCmd>("/" + _rname + "_gazebo/gripper_controller/command", 1);
} }
void IOROS::_initRecv(){ void IOROS::_initRecv(){
_servo_sub[0] = _nm.subscribe("/z1_gazebo/Joint01_controller/state", 1, &IOROS::_joint00Callback, this); _servo_sub[0] = _nm.subscribe("/" + _rname + "_gazebo/Joint01_controller/state", 1, &IOROS::_joint00Callback, this);
_servo_sub[1] = _nm.subscribe("/z1_gazebo/Joint02_controller/state", 1, &IOROS::_joint01Callback, this); _servo_sub[1] = _nm.subscribe("/" + _rname + "_gazebo/Joint02_controller/state", 1, &IOROS::_joint01Callback, this);
_servo_sub[2] = _nm.subscribe("/z1_gazebo/Joint03_controller/state", 1, &IOROS::_joint02Callback, this); _servo_sub[2] = _nm.subscribe("/" + _rname + "_gazebo/Joint03_controller/state", 1, &IOROS::_joint02Callback, this);
_servo_sub[3] = _nm.subscribe("/z1_gazebo/Joint04_controller/state", 1, &IOROS::_joint03Callback, this); _servo_sub[3] = _nm.subscribe("/" + _rname + "_gazebo/Joint04_controller/state", 1, &IOROS::_joint03Callback, this);
_servo_sub[4] = _nm.subscribe("/z1_gazebo/Joint05_controller/state", 1, &IOROS::_joint04Callback, this); _servo_sub[4] = _nm.subscribe("/" + _rname + "_gazebo/Joint05_controller/state", 1, &IOROS::_joint04Callback, this);
_servo_sub[5] = _nm.subscribe("/z1_gazebo/Joint06_controller/state", 1, &IOROS::_joint05Callback, this); _servo_sub[5] = _nm.subscribe("/" + _rname + "_gazebo/Joint06_controller/state", 1, &IOROS::_joint05Callback, this);
_servo_sub[6] = _nm.subscribe("/z1_gazebo/gripper_controller/state", 1, &IOROS::_gripperCallback, this); _servo_sub[6] = _nm.subscribe("/" + _rname + "_gazebo/gripper_controller/state", 1, &IOROS::_gripperCallback, this);
} }
void IOROS::_joint00Callback(const unitree_legged_msgs::MotorState& msg){ 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].dq = msg.dq;
_joint_state[6].ddq = msg.ddq; _joint_state[6].ddq = msg.ddq;
_joint_state[6].tauEst = msg.tauEst; _joint_state[6].tauEst = msg.tauEst;
} }

3
sim/IOROS.h
View File

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