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

nav2 cmd_vel twist interface added to go2py_bridge

This commit is contained in:
Rooholla-KhorramBakht 2024-02-22 13:36:27 -05:00
parent 5747142ac0
commit afc8ba24ac
1 changed files with 18 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

18
deploy/robot_ws/src/go2py_node/src/bridge.cpp
View File

@ -14,6 +14,7 @@
#include "sensor_msgs/msg/temperature.hpp" #include "sensor_msgs/msg/temperature.hpp"
#include "sensor_msgs/msg/battery_state.hpp" #include "sensor_msgs/msg/battery_state.hpp"
#include "geometry_msgs/msg/twist_stamped.hpp" #include "geometry_msgs/msg/twist_stamped.hpp"
#include "geometry_msgs/msg/twist.hpp"
#include "nav_msgs/msg/odometry.hpp" #include "nav_msgs/msg/odometry.hpp"
#include <tf2/LinearMath/Quaternion.h> #include <tf2/LinearMath/Quaternion.h>
@ -34,6 +35,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);
sub_twist = this->create_subscription<geometry_msgs::msg::TwistStamped>("/go2/twist_cmd", 1, std::bind(&Custom::twistCmdCallback, this, std::placeholders::_1)); sub_twist = this->create_subscription<geometry_msgs::msg::TwistStamped>("/go2/twist_cmd", 1, std::bind(&Custom::twistCmdCallback, this, std::placeholders::_1));
nav2_twist = this->create_subscription<geometry_msgs::msg::Twist>("/go2/cmd_vel", 1, std::bind(&Custom::nav2TwistCmdCallback, this, std::placeholders::_1));
// Go2 highlevel subscriber and publishers // Go2 highlevel subscriber and publishers
// the state_suber is set to subscribe "sportmodestate" topic // the state_suber is set to subscribe "sportmodestate" topic
@ -58,8 +60,11 @@ class Custom: public rclcpp::Node
void watchdog(); void watchdog();
// Highlevel twist command through standard ROS2 message type // Highlevel twist command through standard ROS2 message type
void twistCmdCallback(const geometry_msgs::msg::TwistStamped::SharedPtr msg); 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::TwistStamped twist_cmd;
geometry_msgs::msg::Twist twist;
rclcpp::Subscription<geometry_msgs::msg::TwistStamped>::SharedPtr sub_twist; rclcpp::Subscription<geometry_msgs::msg::TwistStamped>::SharedPtr sub_twist;
rclcpp::Subscription<geometry_msgs::msg::Twist>::SharedPtr nav2_twist;
uint64_t last_highcmd_stamp = 0; uint64_t last_highcmd_stamp = 0;
// ROS2 standard joint state, IMU, and odometry publishers // ROS2 standard joint state, IMU, and odometry publishers
@ -256,6 +261,19 @@ void Custom::twistCmdCallback(const geometry_msgs::msg::TwistStamped::SharedPtr
highreq_puber->publish(highreq); 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<std::chrono::microseconds>(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(unitree_go::msg::Go2pyLowCmd::SharedPtr data) void Custom::lowcmd_callback(unitree_go::msg::Go2pyLowCmd::SharedPtr data)
{ {
if(Estop == 0) if(Estop == 0)