Diff: main.cpp

Revision:

3:78a479ae6642

Parent:

2:b7bba9a5f4a5

--- a/main.cpp	Sat Mar 19 16:22:47 2022 +0000
+++ b/main.cpp	Tue Mar 22 17:21:02 2022 +0000
@@ -29,6 +29,38 @@
 
 #define COEFF_LIN 10.0
 #define COEFF_ANG 10.0
+#define CONSOLE_LIN 0.5
+#define CONSOLE_ANG 1.0
+
+geometry_msgs::msg::Twist set_twist_val(
+  double linear_x, double linear_y, double linear_z,
+  double angular_x, double angular_y, double angular_z)
+{
+  geometry_msgs::msg::Vector3 linear;
+  geometry_msgs::msg::Vector3 angular;
+  geometry_msgs::msg::Twist twist;
+
+  linear.x = linear_x;
+  linear.y = linear_y;
+  linear.z = linear_z;
+  angular.x = angular_x;
+  angular.y = angular_y;
+  angular.z = angular_z;
+  twist.linear = linear;
+  twist.angular = angular;
+
+  return twist;
+}
+
+std::string double_to_string(double value)
+{
+  int intpart, fracpart;
+  char str[12];
+  intpart = (int)value;
+  fracpart = (int)((value - intpart) * 10000);
+  sprintf(str, "%d.%04d", intpart, fracpart);
+  return str;
+}
 
 int main() {
   EthernetInterface network;
@@ -37,7 +69,7 @@
   nsapi_size_or_error_t result = network.connect();
 
   printf("mbed mros2 start!\r\n");
-  printf("app name: mturtle_teleop_joy\r\n");
+  printf("app name: mturtle_teleop\r\n");
   mros2::init(0, NULL);
   MROS2_DEBUG("mROS 2 initialization is completed\r\n");
 
@@ -49,62 +81,128 @@
   geometry_msgs::msg::Vector3 linear;
   geometry_msgs::msg::Vector3 angular;
   geometry_msgs::msg::Twist twist;
-  linear.y = 0;
-  linear.z = 0;
-  angular.x = 0;
-  angular.y = 0;
 
-  MROS2_INFO("publish Twist msg to turtlesim according to the input from Joystick module");
-  float initialA = inputA0.read();
   pollfh fds[1];
   fds[0].fh = mbed::mbed_file_handle(STDIN_FILENO);
   fds[0].events = POLLIN;
-  bool console_mode = false;
 
+  auto publish_count = 10;
+  double speed = 0.5;
+  double turn = 1.0;
+  char c;
   while (1)
-  {   
-    if (console_mode || poll(fds, 1, 0))
+  {
+    if (publish_count < 10)
+    {
+      publish_count++;
+    }
+    else
+    {
+      publish_count = 0;
+      MROS2_INFO("keymap to move arround:");
+      MROS2_INFO("------------------");
+      MROS2_INFO("   u    i    o");
+      MROS2_INFO("   j    k    l");
+      MROS2_INFO("   m    ,    .");
+      MROS2_INFO("------------------");
+      MROS2_INFO("q/z : increase/decrease max speeds by 10 percent");
+      MROS2_INFO("w/x : increase/decrease only linear speed by 10 percent");
+      MROS2_INFO("e/c : increase/decrease only angular speed by 10 percent");
+      MROS2_INFO("currently: speed %s / turn %s",
+                 double_to_string(speed).c_str(), double_to_string(turn).c_str());
+      MROS2_INFO("");
+    }
+
+    char c;
+    mbed::mbed_file_handle(STDIN_FILENO)->read(&c, 1);
+    switch (c)
     {
-        console_mode = true;
-        if (poll(fds, 1, 0))
-        {
-           char c;
-            mbed::mbed_file_handle(STDIN_FILENO)->read(&c, 1);
-            switch (c)
-            {
-                case 'w':
-                    linear.x =   COEFF_LIN * 5.0;
-                    break;
-                case 's':
-                    linear.x = 0.0;
-                    angular.z = 0.0;
-                    break;
-                case 'x':
-                    linear.x =   -COEFF_LIN * 5.0;
-                    break;
-                case 'a':
-                    angular.z =   COEFF_ANG * 5.0;
-                    break;
-                case 'd':
-                    angular.z =  -COEFF_ANG * 5.0;
-                    break;
-                case 'q':
-                    linear.x = 0.0;
-                    angular.z = 0.0;
-                    console_mode = false;
-                    break;
-            }
-        }
-    } else {
-        linear.x = COEFF_LIN * (inputA0.read() - initialA);
-        angular.z = COEFF_ANG * (inputA1.read() - initialA);
+    /* increase/decrease speeds */
+    case 'q':
+      speed = speed * 1.1;
+      turn = turn * 1.1;
+      MROS2_INFO("currently: speed %s / turn %s",
+                 double_to_string(speed).c_str(), double_to_string(turn).c_str());
+      break;
+    case 'z':
+      speed = speed * 0.9;
+      turn = turn * 0.9;
+      MROS2_INFO("currently: speed %s / turn %s",
+                 double_to_string(speed).c_str(), double_to_string(turn).c_str());
+      break;
+    case 'w':
+      speed = speed * 1.1;
+      MROS2_INFO("currently: speed %s / turn %s",
+                 double_to_string(speed).c_str(), double_to_string(turn).c_str());
+      break;
+    case 'x':
+      speed = speed * 0.9;
+      MROS2_INFO("currently: speed %s / turn %s",
+                 double_to_string(speed).c_str(), double_to_string(turn).c_str());
+      break;
+    case 'e':
+      turn = turn * 1.1;
+      MROS2_INFO("currently: speed %s / turn %s",
+                 double_to_string(speed).c_str(), double_to_string(turn).c_str());
+      break;
+    case 'c':
+      turn = turn * 0.9;
+      MROS2_INFO("currently: speed %s / turn %s",
+                 double_to_string(speed).c_str(), double_to_string(turn).c_str());
+      break;
+    /* set direction */
+    case 'u':
+      twist = set_twist_val(speed, 0.0, 0.0, 0.0, 0.0, turn);
+      MROS2_INFO("publishing Twist msg by 'u' command");
+      pub.publish(twist);
+      break;
+    case 'i':
+      twist = set_twist_val(speed, 0.0, 0.0, 0.0, 0.0, 0.0);
+      MROS2_INFO("publishing Twist msg by 'i' command");
+      pub.publish(twist);
+      break;
+    case 'o':
+      twist = set_twist_val(speed, 0.0, 0.0, 0.0, 0.0, -turn);
+      MROS2_INFO("publishing Twist msg by 'o' command");
+      pub.publish(twist);
+      break;
+    case 'j':
+      twist = set_twist_val(0.0, 0.0, 0.0, 0.0, 0.0, turn);
+      MROS2_INFO("publishing Twist msg by 'j' command");
+      pub.publish(twist);
+      break;
+    case 'k':
+      twist = set_twist_val(0.0, 0.0, 0.0, 0.0, 0.0, 0.0);
+      MROS2_INFO("publishing Twist msg by 'k' command");
+      pub.publish(twist);
+      break;
+    case 'l':
+      twist = set_twist_val(0.0, 0.0, 0.0, 0.0, 0.0, -turn);
+      MROS2_INFO("publishing Twist msg by 'l' command");
+      pub.publish(twist);
+      break;
+    case 'm':
+      twist = set_twist_val(-speed, 0.0, 0.0, 0.0, 0.0, -turn);
+      MROS2_INFO("publishing Twist msg by 'm' command");
+      pub.publish(twist);
+      break;
+    case ',':
+      twist = set_twist_val(-speed, 0.0, 0.0, 0.0, 0.0, 0.0);
+      MROS2_INFO("publishing Twist msg by ',' command");
+      pub.publish(twist);
+      break;
+    case '.':
+      twist = set_twist_val(-speed, 0.0, 0.0, 0.0, 0.0, turn);
+      MROS2_INFO("publishing Twist msg by '.' command");
+      pub.publish(twist);
+      break;
+    default:
+      MROS2_INFO("ERROR: wrong command");
+      publish_count = 10;
+      break;
     }
-    twist.linear = linear;
-    twist.angular = angular;
-    pub.publish(twist);
-    osDelay(100);
   }
 
   mros2::spin();
   return 0;
-}
\ No newline at end of file
+}