#include "unitree_go/msg/low_state.hpp" #include "unitree_go/msg/imu_state.hpp" #include "unitree_go/msg/motor_state.hpp" #include "go2py_common/msg/go2py_status.hpp" #include "go2py_common/msg/go2py_low_cmd.hpp" #include "unitree_go/msg/low_cmd.hpp" #include "unitree_go/msg/motor_cmd.hpp" #include "unitree_go/msg/bms_cmd.hpp" #include "common/motor_crc.h" #include "rclcpp/rclcpp.hpp" #include "sensor_msgs/msg/imu.hpp" #include "sensor_msgs/msg/joint_state.hpp" #include "sensor_msgs/msg/temperature.hpp" #include "sensor_msgs/msg/battery_state.hpp" #include "geometry_msgs/msg/twist_stamped.hpp" #include "geometry_msgs/msg/twist.hpp" #include "nav_msgs/msg/odometry.hpp" #include // headers needed for highlevel control #include "unitree_go/msg/sport_mode_state.hpp" #include "unitree_api/msg/request.hpp" #include "common/ros2_sport_client.h" #include "joystick.h" #include "unitree_api/msg/request.hpp" class Custom: public rclcpp::Node { public: Custom() : Node("go2py_bridge_node") { watchdog_timer = this->create_wall_timer(std::chrono::milliseconds(10), std::bind(&Custom::watchdog, this)); pub_odom = this->create_publisher("/go2/odom", 1); pub_imu = this->create_publisher("/go2/imu", 1); pub_joint = this->create_publisher("/go2/joint_states", 1); sub_twist = this->create_subscription("/go2/twist_cmd", 1, std::bind(&Custom::twistCmdCallback, this, std::placeholders::_1)); nav2_twist = this->create_subscription("/go2/cmd_vel", 1, std::bind(&Custom::nav2TwistCmdCallback, this, std::placeholders::_1)); // Go2 highlevel subscriber and publishers // the state_suber is set to subscribe "sportmodestate" topic highstate_suber = this->create_subscription( "sportmodestate", 10, std::bind(&Custom::highstate_callback, this, std::placeholders::_1)); // the req_puber is set to subscribe "/api/sport/request" topic with dt highreq_puber = this->create_publisher("/api/sport/request", 1); //Go2 lowlevel interface init_lowcmd(); lowstate_suber = this->create_subscription( "lowstate", 1, std::bind(&Custom::lowstate_callback, this, std::placeholders::_1)); lowcmd_suber = this->create_subscription( "/go2/lowcmd", 1, std::bind(&Custom::lowcmd_callback, this, std::placeholders::_1)); lowcmd_puber = this->create_publisher("/lowcmd", 1); api_publisher = this->create_publisher("/api/robot_state/request", 1); status_publisher = this->create_publisher("/go2py/status", 1); } private: rclcpp::TimerBase::SharedPtr watchdog_timer; void watchdog(); // Highlevel twist command through standard ROS2 message type void twistCmdCallback(const geometry_msgs::msg::TwistStamped::SharedPtr msg); void nav2TwistCmdCallback(const geometry_msgs::msg::Twist::SharedPtr msg); geometry_msgs::msg::TwistStamped twist_cmd; geometry_msgs::msg::Twist twist; rclcpp::Subscription::SharedPtr sub_twist; rclcpp::Subscription::SharedPtr nav2_twist; uint64_t last_highcmd_stamp = 0; // ROS2 standard joint state, IMU, and odometry publishers rclcpp::Publisher::SharedPtr pub_imu; rclcpp::Publisher::SharedPtr pub_joint; rclcpp::Publisher::SharedPtr pub_odom; // Highlevel interface void highstate_callback(unitree_go::msg::SportModeState::SharedPtr data); rclcpp::Subscription::SharedPtr highstate_suber; rclcpp::Publisher::SharedPtr highreq_puber; unitree_api::msg::Request highreq; // Unitree Go2 ROS2 request message SportClient sport_req; // Lowlevel interface void lowstate_callback(unitree_go::msg::LowState::SharedPtr data); void lowcmd_callback(go2py_common::msg::Go2pyLowCmd::SharedPtr data); rclcpp::Subscription::SharedPtr lowstate_suber; rclcpp::Subscription::SharedPtr lowcmd_suber; // A struct to store the highlevel states for later use rclcpp::Publisher::SharedPtr lowcmd_puber; unitree_go::msg::LowCmd lowcmd_msg; uint64_t last_lowcmd_stamp = 0; xRockerBtnDataStruct _keyData; rclcpp::Publisher::SharedPtr api_publisher; rclcpp::Publisher::SharedPtr status_publisher; void init_lowcmd() { for (int i = 0; i < 20; i++) { lowcmd_msg.motor_cmd[i].mode = 0x01; //Set toque mode, 0x00 is passive mode lowcmd_msg.motor_cmd[i].q = PosStopF; lowcmd_msg.motor_cmd[i].kp = 0; lowcmd_msg.motor_cmd[i].dq = VelStopF; lowcmd_msg.motor_cmd[i].kd = 0; lowcmd_msg.motor_cmd[i].tau = 0; } } struct { float px; float py; float height; float roll; float pitch; float yaw; float imu_temp; float wx; float wy; float wz; float ax; float qy; float az; float vx; float vy; float foot_force[4]; }highstate; int Estop = 0; int sport_mode = 1; }; void Custom::lowstate_callback(unitree_go::msg::LowState::SharedPtr data) { // std::cout << data->tick << std::endl; sensor_msgs::msg::Imu imu; sensor_msgs::msg::JointState joint_state; nav_msgs::msg::Odometry odom_state; // Load the IMU message imu.header.stamp=rclcpp::Clock().now(); imu.header.frame_id = "imu_link"; imu.orientation.w = data->imu_state.quaternion[0]; imu.orientation.x = data->imu_state.quaternion[1]; imu.orientation.y = data->imu_state.quaternion[2]; imu.orientation.z = data->imu_state.quaternion[3]; imu.linear_acceleration.x = data->imu_state.accelerometer[0]; imu.linear_acceleration.y = data->imu_state.accelerometer[1]; imu.linear_acceleration.z = data->imu_state.accelerometer[2]; imu.angular_velocity.x = data->imu_state.gyroscope[0]; imu.angular_velocity.y = data->imu_state.gyroscope[1]; imu.angular_velocity.z = data->imu_state.gyroscope[2]; // Load the joint state messages joint_state.header.stamp = imu.header.stamp; joint_state.header.frame_id = "trunk"; joint_state.name.push_back("FR_hip_joint"); joint_state.name.push_back("FR_thigh_joint"); joint_state.name.push_back("FR_calf_joint"); joint_state.name.push_back("FL_hip_joint"); joint_state.name.push_back("FL_thigh_joint"); joint_state.name.push_back("FL_calf_joint"); joint_state.name.push_back("RR_hip_joint"); joint_state.name.push_back("RR_thigh_joint"); joint_state.name.push_back("RR_calf_joint"); joint_state.name.push_back("RL_hip_joint"); joint_state.name.push_back("RL_thigh_joint"); joint_state.name.push_back("RL_calf_joint"); for(int i=0; i<12; i++) { joint_state.position.push_back(data->motor_state[i].q); joint_state.velocity.push_back(data->motor_state[i].dq); joint_state.effort.push_back(data->motor_state[i].tau_est); } pub_joint->publish(joint_state); pub_imu->publish(imu); // Check for emergency stop memcpy(&_keyData, &data->wireless_remote[0], 40); if (_keyData.btn.components.R2 == 1 && _keyData.btn.components.L2 == 1) { Estop = 1; } if ((_keyData.btn.components.L2 == 1 && _keyData.btn.components.A == 1)) { Estop = 0; } if ((_keyData.btn.components.L2 == 1 && _keyData.btn.components.L1 == 1)) { auto msg = std::make_shared(); // Populate the message fields msg->header.identity.id = 80005; msg->header.identity.api_id = 1001; msg->parameter = "{\"name\":\"sport_mode\",\"switch\":0}"; api_publisher->publish(*msg); sport_mode = 0; } if ((_keyData.btn.components.R2 == 1 && _keyData.btn.components.R1 == 1)) { auto msg = std::make_shared(); // Populate the message fields msg->header.identity.id = 80005; msg->header.identity.api_id = 1001; msg->parameter = "{\"name\":\"sport_mode\",\"switch\":1}"; api_publisher->publish(*msg); sport_mode = 1; } // std::cout << "Estop: " << Estop << std::endl; } void Custom::highstate_callback(unitree_go::msg::SportModeState::SharedPtr data) { highstate.px = data->position[0]; highstate.py = data->position[1]; highstate.roll = data->imu_state.rpy[0]; highstate.pitch = data->imu_state.rpy[1]; highstate.yaw = data->imu_state.rpy[2]; for(int i=0; i<4; i++) { highstate.foot_force[i]=data->foot_force[i]; } highstate.height = data->body_height; nav_msgs::msg::Odometry odom_state; // odometry states published by the onboard high-level controller odom_state.header.stamp = rclcpp::Clock().now(); odom_state.header.frame_id = "odom"; odom_state.child_frame_id = "base_link"; odom_state.pose.pose.position.x = data->position[0]; odom_state.pose.pose.position.y = data->position[1]; odom_state.pose.pose.position.z = data->body_height; odom_state.twist.twist.linear.x = data->velocity[0]; odom_state.twist.twist.linear.y = data->velocity[1]; odom_state.twist.twist.linear.z = data->velocity[2]; odom_state.twist.twist.angular.z= data->yaw_speed; double position_R = 0.05; double orientation_R = 0.005; odom_state.pose.covariance = { position_R, 0, 0, 0, 0, 0, // Covariance for position x 0, position_R, 0, 0, 0, 0, // Covariance for position y 0, 0, position_R, 0, 0, 0, // Covariance for position z 0, 0, 0, orientation_R, 0, 0, // Covariance for orientation roll 0, 0, 0, 0, orientation_R, 0, // Covariance for orientation pitch 0, 0, 0, 0, 0, orientation_R // Covariance for orientation yaw }; odom_state.twist.covariance = { position_R, 0, 0, 0, 0, 0, // Covariance for position x 0, position_R, 0, 0, 0, 0, // Covariance for position y 0, 0, position_R, 0, 0, 0, // Covariance for position z 0, 0, 0, orientation_R, 0, 0, // Covariance for orientation roll 0, 0, 0, 0, orientation_R, 0, // Covariance for orientation pitch 0, 0, 0, 0, 0, orientation_R // Covariance for orientation yaw }; pub_odom->publish(odom_state); } void Custom::twistCmdCallback(const geometry_msgs::msg::TwistStamped::SharedPtr msg) { auto stamp_now = std::chrono::high_resolution_clock::now(); last_highcmd_stamp = std::chrono::duration_cast(stamp_now.time_since_epoch()).count(); if(Estop == 0) sport_req.Move(highreq, msg->twist.linear.x, msg->twist.linear.y, msg->twist.angular.z); else sport_req.Damp(highreq); // Publish request messages with desired body velocity highreq_puber->publish(highreq); } void Custom::nav2TwistCmdCallback(const geometry_msgs::msg::Twist::SharedPtr msg) { auto stamp_now = std::chrono::high_resolution_clock::now(); last_highcmd_stamp = std::chrono::duration_cast(stamp_now.time_since_epoch()).count(); if(Estop == 0) sport_req.Move(highreq, msg->linear.x, msg->linear.y, msg->angular.z); else sport_req.Damp(highreq); // Publish request messages with desired body velocity highreq_puber->publish(highreq); } void Custom::lowcmd_callback(go2py_common::msg::Go2pyLowCmd::SharedPtr data) { if(Estop == 0) { for(int i=0; i<12; i++) { lowcmd_msg.motor_cmd[i].q = data->q[i]; // Taregt angular(rad) lowcmd_msg.motor_cmd[i].kp = data->kp[i]; // Poinstion(rad) control kp gain lowcmd_msg.motor_cmd[i].dq = data->dq[i]; // Taregt angular velocity(rad/ss) lowcmd_msg.motor_cmd[i].kd = data->kd[i]; // Poinstion(rad) control kd gain lowcmd_msg.motor_cmd[i].tau = data->tau[i]; // Feedforward toque 1N.m get_crc(lowcmd_msg); //Compute the CRC and load it into the message } }else { for(int i=0; i<12; i++) { lowcmd_msg.motor_cmd[i].q = 0.; // Taregt angular(rad) lowcmd_msg.motor_cmd[i].kp = 0.; // Poinstion(rad) control kp gain lowcmd_msg.motor_cmd[i].dq = 0.; // Taregt angular velocity(rad/ss) lowcmd_msg.motor_cmd[i].kd = 0; // Poinstion(rad) control kd gain lowcmd_msg.motor_cmd[i].tau = 0.; // Feedforward toque 1N.m get_crc(lowcmd_msg); //Compute the CRC and load it into the message } } lowcmd_puber->publish(lowcmd_msg); //Publish lowcmd message } void Custom::watchdog() { if(Estop == 1) { sport_req.Damp(highreq); highreq_puber->publish(highreq); for(int i=0; i<12; i++) { lowcmd_msg.motor_cmd[i].q = 0.; // Taregt angular(rad) lowcmd_msg.motor_cmd[i].kp = 0.; // Poinstion(rad) control kp gain lowcmd_msg.motor_cmd[i].dq = 0.; // Taregt angular velocity(rad/ss) lowcmd_msg.motor_cmd[i].kd = 3; // Poinstion(rad) control kd gain lowcmd_msg.motor_cmd[i].tau = 0.; // Feedforward toque 1N.m get_crc(lowcmd_msg); //Compute the CRC and load it into the message lowcmd_puber->publish(lowcmd_msg); //Publish lowcmd message } lowcmd_puber->publish(lowcmd_msg); } auto msg = std::make_shared(); msg->estop = Estop; msg->sport_mode = sport_mode; status_publisher->publish(*msg); } int main(int argc, char **argv) { rclcpp::init(argc, argv); // Initialize rclcpp rclcpp::spin(std::make_shared()); // Run ROS2 node which is make share with low_state_suber class rclcpp::shutdown(); // Exit return 0; }