249 lines
10 KiB
C++
249 lines
10 KiB
C++
/************************************************************************
|
|
Copyright (c) 2018-2019, Unitree Robotics.Co.Ltd. All rights reserved.
|
|
Use of this source code is governed by the MPL-2.0 license, see LICENSE.
|
|
************************************************************************/
|
|
|
|
#include "ros/ros.h"
|
|
#include <stdio.h>
|
|
#include <stdlib.h>
|
|
#include "unitree_legged_msgs/LowCmd.h"
|
|
#include "unitree_legged_msgs/LowState.h"
|
|
#include "unitree_legged_msgs/MotorCmd.h"
|
|
#include "unitree_legged_msgs/MotorState.h"
|
|
#include <geometry_msgs/WrenchStamped.h>
|
|
#include <sensor_msgs/Imu.h>
|
|
#include <std_msgs/Bool.h>
|
|
#include <vector>
|
|
#include <string>
|
|
#include <math.h>
|
|
#include <nav_msgs/Odometry.h>
|
|
#include "body.h"
|
|
|
|
using namespace std;
|
|
using namespace unitree_model;
|
|
|
|
bool start_up = true;
|
|
|
|
class multiThread
|
|
{
|
|
public:
|
|
multiThread(string rname){
|
|
robot_name = rname;
|
|
imu_sub = nm.subscribe("/trunk_imu", 1, &multiThread::imuCallback, this);
|
|
footForce_sub[0] = nm.subscribe("/visual/FR_foot_contact/the_force", 1, &multiThread::FRfootCallback, this);
|
|
footForce_sub[1] = nm.subscribe("/visual/FL_foot_contact/the_force", 1, &multiThread::FLfootCallback, this);
|
|
footForce_sub[2] = nm.subscribe("/visual/RR_foot_contact/the_force", 1, &multiThread::RRfootCallback, this);
|
|
footForce_sub[3] = nm.subscribe("/visual/RL_foot_contact/the_force", 1, &multiThread::RLfootCallback, this);
|
|
servo_sub[0] = nm.subscribe("/" + robot_name + "_gazebo/FR_hip_controller/state", 1, &multiThread::FRhipCallback, this);
|
|
servo_sub[1] = nm.subscribe("/" + robot_name + "_gazebo/FR_thigh_controller/state", 1, &multiThread::FRthighCallback, this);
|
|
servo_sub[2] = nm.subscribe("/" + robot_name + "_gazebo/FR_calf_controller/state", 1, &multiThread::FRcalfCallback, this);
|
|
servo_sub[3] = nm.subscribe("/" + robot_name + "_gazebo/FL_hip_controller/state", 1, &multiThread::FLhipCallback, this);
|
|
servo_sub[4] = nm.subscribe("/" + robot_name + "_gazebo/FL_thigh_controller/state", 1, &multiThread::FLthighCallback, this);
|
|
servo_sub[5] = nm.subscribe("/" + robot_name + "_gazebo/FL_calf_controller/state", 1, &multiThread::FLcalfCallback, this);
|
|
servo_sub[6] = nm.subscribe("/" + robot_name + "_gazebo/RR_hip_controller/state", 1, &multiThread::RRhipCallback, this);
|
|
servo_sub[7] = nm.subscribe("/" + robot_name + "_gazebo/RR_thigh_controller/state", 1, &multiThread::RRthighCallback, this);
|
|
servo_sub[8] = nm.subscribe("/" + robot_name + "_gazebo/RR_calf_controller/state", 1, &multiThread::RRcalfCallback, this);
|
|
servo_sub[9] = nm.subscribe("/" + robot_name + "_gazebo/RL_hip_controller/state", 1, &multiThread::RLhipCallback, this);
|
|
servo_sub[10] = nm.subscribe("/" + robot_name + "_gazebo/RL_thigh_controller/state", 1, &multiThread::RLthighCallback, this);
|
|
servo_sub[11] = nm.subscribe("/" + robot_name + "_gazebo/RL_calf_controller/state", 1, &multiThread::RLcalfCallback, this);
|
|
}
|
|
|
|
void imuCallback(const sensor_msgs::Imu & msg)
|
|
{
|
|
lowState.imu.quaternion[0] = msg.orientation.w;
|
|
lowState.imu.quaternion[1] = msg.orientation.x;
|
|
lowState.imu.quaternion[2] = msg.orientation.y;
|
|
lowState.imu.quaternion[3] = msg.orientation.z;
|
|
|
|
lowState.imu.gyroscope[0] = msg.angular_velocity.x;
|
|
lowState.imu.gyroscope[1] = msg.angular_velocity.y;
|
|
lowState.imu.gyroscope[2] = msg.angular_velocity.z;
|
|
|
|
lowState.imu.accelerometer[0] = msg.linear_acceleration.x;
|
|
lowState.imu.accelerometer[1] = msg.linear_acceleration.y;
|
|
lowState.imu.accelerometer[2] = msg.linear_acceleration.z;
|
|
|
|
}
|
|
|
|
void FRhipCallback(const unitree_legged_msgs::MotorState& msg)
|
|
{
|
|
start_up = false;
|
|
lowState.motorState[0].mode = msg.mode;
|
|
lowState.motorState[0].q = msg.q;
|
|
lowState.motorState[0].dq = msg.dq;
|
|
lowState.motorState[0].tauEst = msg.tauEst;
|
|
}
|
|
|
|
void FRthighCallback(const unitree_legged_msgs::MotorState& msg)
|
|
{
|
|
lowState.motorState[1].mode = msg.mode;
|
|
lowState.motorState[1].q = msg.q;
|
|
lowState.motorState[1].dq = msg.dq;
|
|
lowState.motorState[1].tauEst = msg.tauEst;
|
|
}
|
|
|
|
void FRcalfCallback(const unitree_legged_msgs::MotorState& msg)
|
|
{
|
|
lowState.motorState[2].mode = msg.mode;
|
|
lowState.motorState[2].q = msg.q;
|
|
lowState.motorState[2].dq = msg.dq;
|
|
lowState.motorState[2].tauEst = msg.tauEst;
|
|
}
|
|
|
|
void FLhipCallback(const unitree_legged_msgs::MotorState& msg)
|
|
{
|
|
start_up = false;
|
|
lowState.motorState[3].mode = msg.mode;
|
|
lowState.motorState[3].q = msg.q;
|
|
lowState.motorState[3].dq = msg.dq;
|
|
lowState.motorState[3].tauEst = msg.tauEst;
|
|
}
|
|
|
|
void FLthighCallback(const unitree_legged_msgs::MotorState& msg)
|
|
{
|
|
lowState.motorState[4].mode = msg.mode;
|
|
lowState.motorState[4].q = msg.q;
|
|
lowState.motorState[4].dq = msg.dq;
|
|
lowState.motorState[4].tauEst = msg.tauEst;
|
|
}
|
|
|
|
void FLcalfCallback(const unitree_legged_msgs::MotorState& msg)
|
|
{
|
|
lowState.motorState[5].mode = msg.mode;
|
|
lowState.motorState[5].q = msg.q;
|
|
lowState.motorState[5].dq = msg.dq;
|
|
lowState.motorState[5].tauEst = msg.tauEst;
|
|
}
|
|
|
|
void RRhipCallback(const unitree_legged_msgs::MotorState& msg)
|
|
{
|
|
start_up = false;
|
|
lowState.motorState[6].mode = msg.mode;
|
|
lowState.motorState[6].q = msg.q;
|
|
lowState.motorState[6].dq = msg.dq;
|
|
lowState.motorState[6].tauEst = msg.tauEst;
|
|
}
|
|
|
|
void RRthighCallback(const unitree_legged_msgs::MotorState& msg)
|
|
{
|
|
lowState.motorState[7].mode = msg.mode;
|
|
lowState.motorState[7].q = msg.q;
|
|
lowState.motorState[7].dq = msg.dq;
|
|
lowState.motorState[7].tauEst = msg.tauEst;
|
|
}
|
|
|
|
void RRcalfCallback(const unitree_legged_msgs::MotorState& msg)
|
|
{
|
|
lowState.motorState[8].mode = msg.mode;
|
|
lowState.motorState[8].q = msg.q;
|
|
lowState.motorState[8].dq = msg.dq;
|
|
lowState.motorState[8].tauEst = msg.tauEst;
|
|
}
|
|
|
|
void RLhipCallback(const unitree_legged_msgs::MotorState& msg)
|
|
{
|
|
start_up = false;
|
|
lowState.motorState[9].mode = msg.mode;
|
|
lowState.motorState[9].q = msg.q;
|
|
lowState.motorState[9].dq = msg.dq;
|
|
lowState.motorState[9].tauEst = msg.tauEst;
|
|
}
|
|
|
|
void RLthighCallback(const unitree_legged_msgs::MotorState& msg)
|
|
{
|
|
lowState.motorState[10].mode = msg.mode;
|
|
lowState.motorState[10].q = msg.q;
|
|
lowState.motorState[10].dq = msg.dq;
|
|
lowState.motorState[10].tauEst = msg.tauEst;
|
|
}
|
|
|
|
void RLcalfCallback(const unitree_legged_msgs::MotorState& msg)
|
|
{
|
|
lowState.motorState[11].mode = msg.mode;
|
|
lowState.motorState[11].q = msg.q;
|
|
lowState.motorState[11].dq = msg.dq;
|
|
lowState.motorState[11].tauEst = msg.tauEst;
|
|
}
|
|
|
|
void FRfootCallback(const geometry_msgs::WrenchStamped& msg)
|
|
{
|
|
lowState.eeForce[0].x = msg.wrench.force.x;
|
|
lowState.eeForce[0].y = msg.wrench.force.y;
|
|
lowState.eeForce[0].z = msg.wrench.force.z;
|
|
lowState.footForce[0] = msg.wrench.force.z;
|
|
}
|
|
|
|
void FLfootCallback(const geometry_msgs::WrenchStamped& msg)
|
|
{
|
|
lowState.eeForce[1].x = msg.wrench.force.x;
|
|
lowState.eeForce[1].y = msg.wrench.force.y;
|
|
lowState.eeForce[1].z = msg.wrench.force.z;
|
|
lowState.footForce[1] = msg.wrench.force.z;
|
|
}
|
|
|
|
void RRfootCallback(const geometry_msgs::WrenchStamped& msg)
|
|
{
|
|
lowState.eeForce[2].x = msg.wrench.force.x;
|
|
lowState.eeForce[2].y = msg.wrench.force.y;
|
|
lowState.eeForce[2].z = msg.wrench.force.z;
|
|
lowState.footForce[2] = msg.wrench.force.z;
|
|
}
|
|
|
|
void RLfootCallback(const geometry_msgs::WrenchStamped& msg)
|
|
{
|
|
lowState.eeForce[3].x = msg.wrench.force.x;
|
|
lowState.eeForce[3].y = msg.wrench.force.y;
|
|
lowState.eeForce[3].z = msg.wrench.force.z;
|
|
lowState.footForce[3] = msg.wrench.force.z;
|
|
}
|
|
|
|
private:
|
|
ros::NodeHandle nm;
|
|
ros::Subscriber servo_sub[12], footForce_sub[4], imu_sub;
|
|
string robot_name;
|
|
};
|
|
|
|
int main(int argc, char **argv)
|
|
{
|
|
ros::init(argc, argv, "unitree_gazebo_servo");
|
|
|
|
string robot_name;
|
|
ros::param::get("/robot_name", robot_name);
|
|
cout << "robot_name: " << robot_name << endl;
|
|
|
|
multiThread listen_publish_obj(robot_name);
|
|
ros::AsyncSpinner spinner(1); // one threads
|
|
spinner.start();
|
|
usleep(300000); // must wait 300ms, to get first state
|
|
|
|
ros::NodeHandle n;
|
|
ros::Publisher lowState_pub; //for rviz visualization
|
|
// ros::Rate loop_rate(1000);
|
|
// the following nodes have been initialized by "gazebo.launch"
|
|
lowState_pub = n.advertise<unitree_legged_msgs::LowState>("/" + robot_name + "_gazebo/lowState/state", 1);
|
|
servo_pub[0] = n.advertise<unitree_legged_msgs::MotorCmd>("/" + robot_name + "_gazebo/FR_hip_controller/command", 1);
|
|
servo_pub[1] = n.advertise<unitree_legged_msgs::MotorCmd>("/" + robot_name + "_gazebo/FR_thigh_controller/command", 1);
|
|
servo_pub[2] = n.advertise<unitree_legged_msgs::MotorCmd>("/" + robot_name + "_gazebo/FR_calf_controller/command", 1);
|
|
servo_pub[3] = n.advertise<unitree_legged_msgs::MotorCmd>("/" + robot_name + "_gazebo/FL_hip_controller/command", 1);
|
|
servo_pub[4] = n.advertise<unitree_legged_msgs::MotorCmd>("/" + robot_name + "_gazebo/FL_thigh_controller/command", 1);
|
|
servo_pub[5] = n.advertise<unitree_legged_msgs::MotorCmd>("/" + robot_name + "_gazebo/FL_calf_controller/command", 1);
|
|
servo_pub[6] = n.advertise<unitree_legged_msgs::MotorCmd>("/" + robot_name + "_gazebo/RR_hip_controller/command", 1);
|
|
servo_pub[7] = n.advertise<unitree_legged_msgs::MotorCmd>("/" + robot_name + "_gazebo/RR_thigh_controller/command", 1);
|
|
servo_pub[8] = n.advertise<unitree_legged_msgs::MotorCmd>("/" + robot_name + "_gazebo/RR_calf_controller/command", 1);
|
|
servo_pub[9] = n.advertise<unitree_legged_msgs::MotorCmd>("/" + robot_name + "_gazebo/RL_hip_controller/command", 1);
|
|
servo_pub[10] = n.advertise<unitree_legged_msgs::MotorCmd>("/" + robot_name + "_gazebo/RL_thigh_controller/command", 1);
|
|
servo_pub[11] = n.advertise<unitree_legged_msgs::MotorCmd>("/" + robot_name + "_gazebo/RL_calf_controller/command", 1);
|
|
|
|
motion_init();
|
|
|
|
while (ros::ok()){
|
|
/*
|
|
control logic
|
|
*/
|
|
lowState_pub.publish(lowState);
|
|
sendServoCmd();
|
|
|
|
}
|
|
return 0;
|
|
}
|