From a33d483ac76b6341d560fc00ce5b01a53c42c9fc Mon Sep 17 00:00:00 2001
From: Rooholla-KhorramBakht <learn.kho@gmail.com>
Date: Fri, 31 May 2024 01:12:26 +0800
Subject: [PATCH] xacro bug fix and adding odometry publisher to the bridge

---
 .../go2_description/xacro/robot.xacro         |  2 +-
 deploy/ros2_nodes/go2py_bridge/src/bridge.cpp | 26 +++++++++++++++++++
 .../src/manager/source_driver_ros2.hpp        |  2 +-
 3 files changed, 28 insertions(+), 2 deletions(-)

diff --git a/deploy/ros2_nodes/go2_description/xacro/robot.xacro b/deploy/ros2_nodes/go2_description/xacro/robot.xacro
index 16a650b..8494b8d 100755
--- a/deploy/ros2_nodes/go2_description/xacro/robot.xacro
+++ b/deploy/ros2_nodes/go2_description/xacro/robot.xacro
@@ -127,7 +127,7 @@
 
     <joint name="infra2_joint" type="fixed">
         <parent link="trunk"/>
-        <child link="infra1_link"/>
+        <child link="infra2_link"/>
         <origin rpy="-1.640637 -0.014093 -1.611177" xyz="0.317969 -0.061919 0.082773"/>
     </joint>
 
diff --git a/deploy/ros2_nodes/go2py_bridge/src/bridge.cpp b/deploy/ros2_nodes/go2py_bridge/src/bridge.cpp
index 943cd3d..d8cf646 100644
--- a/deploy/ros2_nodes/go2py_bridge/src/bridge.cpp
+++ b/deploy/ros2_nodes/go2py_bridge/src/bridge.cpp
@@ -44,6 +44,8 @@ class Custom: public rclcpp::Node
             init_lowcmd();
             unitree_lowstate_suber = this->create_subscription<unitree_go::msg::LowState>(
             "lowstate", 1, std::bind(&Custom::unitree_lowstate_callback, this, std::placeholders::_1));
+            utlidar_odom_subr = this->create_subscription<nav_msgs::msg::Odometry>(
+            "/utlidar/robot_odom", 1, std::bind(&Custom::unitree_odom_callback, this, std::placeholders::_1));
 
             // the req_puber is set to subscribe "/api/sport/request" topic with dt
             unitree_highreq_puber = this->create_publisher<unitree_api::msg::Request>("/api/sport/request", 1);
@@ -70,6 +72,7 @@ class Custom: public rclcpp::Node
         geometry_msgs::msg::TwistStamped twist_cmd;
         void nav2TwistCmdCallback(const geometry_msgs::msg::Twist::SharedPtr msg);
         rclcpp::Subscription<geometry_msgs::msg::Twist>::SharedPtr nav2_twist_subr;        
+        rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr utlidar_odom_subr;        
         rclcpp::Publisher<sensor_msgs::msg::Imu>::SharedPtr pub_imu;
         rclcpp::Publisher<sensor_msgs::msg::JointState>::SharedPtr pub_joint;
         rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr pub_odom;
@@ -79,6 +82,7 @@ class Custom: public rclcpp::Node
         unitree_go::msg::LowCmd lowcmd_msg;
         SportClient sport_req;
         void unitree_lowstate_callback(unitree_go::msg::LowState::SharedPtr data);
+        void unitree_odom_callback(nav_msgs::msg::Odometry::SharedPtr data);
         rclcpp::Subscription<unitree_go::msg::LowState>::SharedPtr unitree_lowstate_suber;
         rclcpp::Publisher<unitree_api::msg::Request>::SharedPtr unitree_highreq_puber;
         rclcpp::Publisher<unitree_go::msg::LowCmd>::SharedPtr lowcmd_puber;
@@ -108,6 +112,28 @@ void Custom::init_lowcmd()
     }
 }
 
+void Custom::unitree_odom_callback(nav_msgs::msg::Odometry::SharedPtr data)
+{
+    nav_msgs::msg::Odometry odom_state;
+    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->pose.pose.position.x;
+    odom_state.pose.pose.position.y = data->pose.pose.position.y;
+    odom_state.pose.pose.position.z = data->pose.pose.position.z;
+    odom_state.pose.pose.orientation.x = data->pose.pose.orientation.x;
+    odom_state.pose.pose.orientation.y = data->pose.pose.orientation.y;
+    odom_state.pose.pose.orientation.z = data->pose.pose.orientation.z;
+    odom_state.pose.pose.orientation.w = data->pose.pose.orientation.w;
+    odom_state.twist.twist.linear.x = data->twist.twist.linear.x;
+    odom_state.twist.twist.linear.y = data->twist.twist.linear.y;
+    odom_state.twist.twist.linear.z = data->twist.twist.linear.z;
+    odom_state.twist.twist.angular.x = data->twist.twist.angular.x;
+    odom_state.twist.twist.angular.y = data->twist.twist.angular.y;
+    odom_state.twist.twist.angular.z = data->twist.twist.angular.z;
+    pub_odom->publish(odom_state);
+}
+
 void Custom::unitree_lowstate_callback(unitree_go::msg::LowState::SharedPtr data)
 {
     sensor_msgs::msg::Imu imu;
diff --git a/deploy/ros2_nodes/lidar_node/src/manager/source_driver_ros2.hpp b/deploy/ros2_nodes/lidar_node/src/manager/source_driver_ros2.hpp
index a936a4a..e740031 100644
--- a/deploy/ros2_nodes/lidar_node/src/manager/source_driver_ros2.hpp
+++ b/deploy/ros2_nodes/lidar_node/src/manager/source_driver_ros2.hpp
@@ -301,7 +301,7 @@ inline void SourceDriver::publishLaserScan(
   scan_msg->range_min = 0.2;
   scan_msg->range_max = 100;
   float max_height_ = 1.0;
-  float min_height_ = 0.;
+  float min_height_ = -0.33;
   
 
   // determine amount of rays to create