EmbodiedAI
/
Go2Py_SIM
mirror of https://github.com/machines-in-motion/Go2Py.git
3
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

ROS foot contact state publisher is added

This commit is contained in:
Rooholla-KhorramBakht 2024-06-21 18:54:48 -04:00
parent 5091445de7
commit c3bc387408
1 changed files with 14 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

15
deploy/ros2_nodes/go2py_bridge/src/bridge.cpp
View File

@ -39,7 +39,7 @@ class Custom: public rclcpp::Node
pub_imu = this->create_publisher<sensor_msgs::msg::Imu>("/go2/imu", 1); pub_imu = this->create_publisher<sensor_msgs::msg::Imu>("/go2/imu", 1);
pub_joint = this->create_publisher<sensor_msgs::msg::JointState>("/go2/joint_states", 1); pub_joint = this->create_publisher<sensor_msgs::msg::JointState>("/go2/joint_states", 1);
nav2_twist_subr = this->create_subscription<geometry_msgs::msg::Twist>("/go2/cmd_vel", 1, std::bind(&Custom::nav2TwistCmdCallback, this, std::placeholders::_1)); nav2_twist_subr = this->create_subscription<geometry_msgs::msg::Twist>("/go2/cmd_vel", 1, std::bind(&Custom::nav2TwistCmdCallback, this, std::placeholders::_1));
pub_foot_contact = this->create_publisher<sensor_msgs::msg::JointState>("/go2/foot_contact", 1);
//Unitree Topics //Unitree Topics
init_lowcmd(); init_lowcmd();
unitree_lowstate_suber = this->create_subscription<unitree_go::msg::LowState>( unitree_lowstate_suber = this->create_subscription<unitree_go::msg::LowState>(
@ -76,6 +76,7 @@ class Custom: public rclcpp::Node
rclcpp::Publisher<sensor_msgs::msg::Imu>::SharedPtr pub_imu; rclcpp::Publisher<sensor_msgs::msg::Imu>::SharedPtr pub_imu;
rclcpp::Publisher<sensor_msgs::msg::JointState>::SharedPtr pub_joint; rclcpp::Publisher<sensor_msgs::msg::JointState>::SharedPtr pub_joint;
rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr pub_odom; rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr pub_odom;
rclcpp::Publisher<sensor_msgs::msg::JointState>::SharedPtr pub_foot_contact;
// Unitree Interface // Unitree Interface
unitree_api::msg::Request highreq; unitree_api::msg::Request highreq;
@ -138,6 +139,7 @@ void Custom::unitree_lowstate_callback(unitree_go::msg::LowState::SharedPtr data
{ {
sensor_msgs::msg::Imu imu; sensor_msgs::msg::Imu imu;
sensor_msgs::msg::JointState joint_state; sensor_msgs::msg::JointState joint_state;
sensor_msgs::msg::JointState foot_contact_state;
nav_msgs::msg::Odometry odom_state; nav_msgs::msg::Odometry odom_state;
go2py_messages::msg::Go2pyState go2py_state; go2py_messages::msg::Go2pyState go2py_state;
@ -197,13 +199,24 @@ void Custom::unitree_lowstate_callback(unitree_go::msg::LowState::SharedPtr data
go2py_state.tau[i]=data->motor_state[i].tau_est; go2py_state.tau[i]=data->motor_state[i].tau_est;
go2py_state.motor_temp[i] = data->motor_state[i].temperature; go2py_state.motor_temp[i] = data->motor_state[i].temperature;
} }
foot_contact_state.header.stamp = imu.header.stamp;
foot_contact_state.header.frame_id = "trunk";
foot_contact_state.name.push_back("FR_contact");
foot_contact_state.name.push_back("FL_contact");
foot_contact_state.name.push_back("RR_contact");
foot_contact_state.name.push_back("RL_contact");
for(int i=0; i<4; i++) for(int i=0; i<4; i++)
{
go2py_state.contact[i]=data->foot_force[i]; go2py_state.contact[i]=data->foot_force[i];
foot_contact_state.effort.push_back(data->foot_force[i]);
}
for(int i=0; i<40; i++) for(int i=0; i<40; i++)
go2py_state.wireless_remote[i]=data->wireless_remote[i]; go2py_state.wireless_remote[i]=data->wireless_remote[i];
go2py_state.soc = data->bms_state.soc; go2py_state.soc = data->bms_state.soc;
pub_joint->publish(joint_state); pub_joint->publish(joint_state);
pub_foot_contact->publish(foot_contact_state);
pub_imu->publish(imu); pub_imu->publish(imu);
go2py_state_puber->publish(go2py_state); go2py_state_puber->publish(go2py_state);
// Check for emergency stop // Check for emergency stop