From 539c3b0b7bf9fdb9c40d232035c796b07f308035 Mon Sep 17 00:00:00 2001
From: GeoChoi <snowgeo@gmail.com>
Date: Fri, 16 Jun 2023 01:18:54 +0900
Subject: [PATCH] Feat : #1 cmd_vel_converter revise

convert cmd_vel to cmd_vel_converter with P gain
---
 src/cmd_vel_converter.cpp | 27 +++++++++++++++++++++++++--
 1 file changed, 25 insertions(+), 2 deletions(-)

diff --git a/src/cmd_vel_converter.cpp b/src/cmd_vel_converter.cpp
index 26335c7..49d7677 100644
--- a/src/cmd_vel_converter.cpp
+++ b/src/cmd_vel_converter.cpp
@@ -17,6 +17,10 @@
 #define MAX_ANGLE 20
 #define MIN_ANGLE 0
 
+
+#define P_VEL 40
+#define P_ANG 20
+
 // Create a publisher for the cmd_vel topic
 ros::Publisher cmd_vel_converter;
 ros::Publisher cmd_vel_echo_converter;
@@ -184,6 +188,8 @@ bool cmdVelEcho2Converted(const geometry_msgs::Twist::ConstPtr &cmd_vel_echo)
     return true;
 }
 
+
+
 /* =====================
 * CallBack Functions
 ===================== */
@@ -235,6 +241,23 @@ void cmdVelCallback(const geometry_msgs::Twist::ConstPtr &cmd_vel)
     }
 }
 
+void cmdVelEcho_simple_Callback(const geometry_msgs::Twist::ConstPtr &cmd_vel_echo)
+{
+    echo_converted.linear.x = (cmd_vel_echo->linear.x)/P_VEL;
+    echo_converted.angular.z = (cmd_vel_echo->angular.z)/P_ANG;
+   
+    cmd_vel_echo_converter.publish(echo_converted);
+}
+
+void cmdVel_simple_Callback(const geometry_msgs::Twist::ConstPtr &cmd_vel)
+{
+    converted.linear.x = P_VEL * cmd_vel->linear.x;
+    converted.angular.z = P_ANG * cmd_vel->angular.z;
+
+    cmd_vel_converter.publish(converted);
+}
+
+
 /* =====================
 * main
 ===================== */
@@ -252,11 +275,11 @@ int main(int argc, char **argv)
     // ROS_INFO("cmd_vel_echo_converter - Setup publisher on cmd_vel_converted");
 
     // Subscribe to the cmd_vel topic
-    cmd_vel_sub = nh.subscribe("cmd_vel", 10, cmdVelCallback);
+    cmd_vel_sub = nh.subscribe("cmd_vel", 10, cmdVelEcho_simple_Callback);
     // ROS_INFO("cmd_vel_converter - Setup subscriber on cmd_vel");
 
     // Subscribe to the cmd_vel topic
-    cmd_vel_echo_sub = nh.subscribe("cmd_vel_echo", 10, cmdVelEchoCallback);
+    cmd_vel_echo_sub = nh.subscribe("cmd_vel_echo", 10, cmdVel_simple_Callback);
     // ROS_INFO("cmd_vel_echo_converter - Setup subscriber on cmd_vel");
 
     // Enter the main event loop