Feat : #1 cmd_vel_converter revise

convert cmd_vel to cmd_vel_converter with P gain

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