Merge pull request #349 from ROBOTIS-GIT/develop

Develop
ROBOTIS-GIT · Dec 8, 2021 · 09e31a8 · 09e31a8
2 parents 5013826 + f25532b
commit 09e31a8
Show file tree

Hide file tree

Showing 15 changed files with 321 additions and 177 deletions.
diff --git a/.github/workflows/ros-ci.yml b/.github/workflows/ros-ci.yml
@@ -3,9 +3,9 @@ name: ros-ci
 # Controls when the action will run. Triggers the workflow on push or pull request
 on:
   push:
-    branches: [ master, develop, kinetic-devel, melodic-devel, noetic-devel ]
+    branches: [ master, develop, melodic-devel, noetic-devel ]
   pull_request:
-    branches: [ master, develop, kinetic-devel, melodic-devel, noetic-devel ]
+    branches: [ master, develop, melodic-devel, noetic-devel ]
 
 # A workflow run is made up of one or more jobs that can run sequentially or in parallel
 jobs:
@@ -15,14 +15,9 @@ jobs:
       fail-fast: false
       matrix:
         ros_distribution:
-          - kinetic
           - melodic
           - noetic
         include:
-            # Kinetic Kame (May 2016 - May 2021)
-          - docker_image: ubuntu:xenial
-            ros_distribution: kinetic
-            ros_version: 1
             # Melodic Morenia (May 2018 - May 2023)
           - docker_image: ubuntu:bionic
             ros_distribution: melodic

diff --git a/dynamixel_workbench_controllers/CMakeLists.txt b/dynamixel_workbench_controllers/CMakeLists.txt
@@ -4,8 +4,8 @@
 cmake_minimum_required(VERSION 3.0.2)
 project(dynamixel_workbench_controllers)
 
-## Compile as C++11, supported in ROS Kinetic and newer
-add_compile_options(-std=c++11)
+## Compile as C++14, supported in ROS Melodic and newer
+add_compile_options(-std=c++14)
 
 ################################################################################
 # Find catkin packages and libraries for catkin and system dependencies

diff --git a/dynamixel_workbench_controllers/src/dynamixel_workbench_controllers.cpp b/dynamixel_workbench_controllers/src/dynamixel_workbench_controllers.cpp
@@ -285,6 +285,8 @@ bool DynamixelController::getPresentPosition(std::vector<std::string> dxl_name)
       for(uint8_t index = 0; index < id_cnt; index++)
       {
         wp.position = dxl_wb_->convertValue2Radian(id_array[index], get_position[index]);
+        wp.velocity = 0.0f;
+        wp.acceleration = 0.0f;
         pre_goal_.push_back(wp);
       }
     }
@@ -306,6 +308,8 @@ bool DynamixelController::getPresentPosition(std::vector<std::string> dxl_name)
       }
 
       wp.position = dxl_wb_->convertValue2Radian((uint8_t)dxl.second, read_position);
+      wp.velocity = 0.0f;
+      wp.acceleration = 0.0f;
       pre_goal_.push_back(wp);
     }
   }

diff --git a/dynamixel_workbench_controllers/src/find_dynamixel.cpp b/dynamixel_workbench_controllers/src/find_dynamixel.cpp
@@ -54,31 +54,33 @@ int main(int argc, char *argv[])
     if (result == false)
     {
       ROS_WARN("%s", log);
-      ROS_WARN("Failed to init");
+      ROS_WARN("Failed to init(%d)", baudrate[index]);
     }
     else
     {
-      ROS_INFO("Succeed to init(%d)", baudrate[index]);
+        ROS_INFO("Succeed to init(%d)", baudrate[index]);
+
+
+      dxl_cnt = 0;
+      for (uint8_t num = 0; num < 100; num++) scanned_id[num] = 0;
+
+      ROS_INFO("Wait for scanning...");
+      result = dxl_wb.scan(scanned_id, &dxl_cnt, range, &log);
+      if (result == false)
+      {
+        ROS_WARN("%s", log);
+        ROS_WARN("Failed to scan");
+      }
+      else
+      {
+        ROS_INFO("Find %d Dynamixels", dxl_cnt);
+
+        for (int cnt = 0; cnt < dxl_cnt; cnt++)
+          ROS_INFO("id : %d, model name : %s", scanned_id[cnt], dxl_wb.getModelName(scanned_id[cnt]));
+      }
     }
-
-    dxl_cnt = 0;
-    for (uint8_t num = 0; num < 100; num++) scanned_id[num] = 0;
-
-    ROS_INFO("Wait for scanning...");
-    result = dxl_wb.scan(scanned_id, &dxl_cnt, range, &log);
-    if (result == false)
-    {
-      ROS_WARN("%s", log);
-      ROS_WARN("Failed to scan");
-    }
-    else
-    {
-      ROS_INFO("Find %d Dynamixels", dxl_cnt);
-
-      for (int cnt = 0; cnt < dxl_cnt; cnt++)
-        ROS_INFO("id : %d, model name : %s", scanned_id[cnt], dxl_wb.getModelName(scanned_id[cnt]));
-    }
-
+
+    dxl_wb.~DynamixelWorkbench();
     index++;
   }
 

diff --git a/dynamixel_workbench_operators/CMakeLists.txt b/dynamixel_workbench_operators/CMakeLists.txt
@@ -4,8 +4,8 @@
 cmake_minimum_required(VERSION 3.0.2)
 project(dynamixel_workbench_operators)
 
-## Compile as C++11, supported in ROS Kinetic and newer
-add_compile_options(-std=c++11)
+## Compile as C++14, supported in ROS Melodic and newer
+add_compile_options(-std=c++14)
 
 ################################################################################
 # Find catkin packages and libraries for catkin and system dependencies

diff --git a/dynamixel_workbench_operators/src/wheel_operator.cpp b/dynamixel_workbench_operators/src/wheel_operator.cpp
@@ -16,8 +16,12 @@
 
 /* Authors: Taehun Lim (Darby) */
 
-#include <fcntl.h>          // FILE control
-#include <termios.h>        // Terminal IO
+#if defined(__linux__) || defined(__APPLE__)
+  #include <fcntl.h>          // FILE control
+  #include <termios.h>        // Terminal IO
+#elif defined(_WIN32)
+  #include <conio.h>
+#endif
 
 #include <ros/ros.h>
 #include <geometry_msgs/Twist.h>
@@ -31,45 +35,55 @@
 
 int getch(void)
 {
-  struct termios oldt, newt;
-  int ch;
-
-  tcgetattr( STDIN_FILENO, &oldt );
-  newt = oldt;
-  newt.c_lflag &= ~(ICANON | ECHO);
-  newt.c_cc[VMIN] = 0;
-  newt.c_cc[VTIME] = 1;
-  tcsetattr( STDIN_FILENO, TCSANOW, &newt );
-  ch = getchar();
-  tcsetattr( STDIN_FILENO, TCSANOW, &oldt );
-
-  return ch;
+  #if defined(__linux__) || defined(__APPLE__)
+
+    struct termios oldt, newt;
+    int ch;
+
+    tcgetattr( STDIN_FILENO, &oldt );
+    newt = oldt;
+    newt.c_lflag &= ~(ICANON | ECHO);
+    newt.c_cc[VMIN] = 0;
+    newt.c_cc[VTIME] = 1;
+    tcsetattr( STDIN_FILENO, TCSANOW, &newt );
+    ch = getchar();
+    tcsetattr( STDIN_FILENO, TCSANOW, &oldt );
+
+    return ch;
+
+  #elif defined(_WIN32) || defined(_WIN64)
+    return _getch();
+  #endif
 }
 
 int kbhit(void)
 {
-  struct termios oldt, newt;
-  int ch;
-  int oldf;
+  #if defined(__linux__) || defined(__APPLE__)
+    struct termios oldt, newt;
+    int ch;
+    int oldf;
 
-  tcgetattr(STDIN_FILENO, &oldt);
-  newt = oldt;
-  newt.c_lflag &= ~(ICANON | ECHO);
-  tcsetattr(STDIN_FILENO, TCSANOW, &newt);
-  oldf = fcntl(STDIN_FILENO, F_GETFL, 0);
-  fcntl(STDIN_FILENO, F_SETFL, oldf | O_NONBLOCK);
+    tcgetattr(STDIN_FILENO, &oldt);
+    newt = oldt;
+    newt.c_lflag &= ~(ICANON | ECHO);
+    tcsetattr(STDIN_FILENO, TCSANOW, &newt);
+    oldf = fcntl(STDIN_FILENO, F_GETFL, 0);
+    fcntl(STDIN_FILENO, F_SETFL, oldf | O_NONBLOCK);
 
-  ch = getchar();
+    ch = getchar();
 
-  tcsetattr(STDIN_FILENO, TCSANOW, &oldt);
-  fcntl(STDIN_FILENO, F_SETFL, oldf);
+    tcsetattr(STDIN_FILENO, TCSANOW, &oldt);
+    fcntl(STDIN_FILENO, F_SETFL, oldf);
 
-  if (ch != EOF)
-  {
-    ungetc(ch, stdin);
-    return 1;
-  }
-  return 0;
+    if (ch != EOF)
+    {
+      ungetc(ch, stdin);
+      return 1;
+    }
+    return 0;
+  #elif defined(_WIN32)
+    return _kbhit();
+  #endif
 }
 
 int main(int argc, char **argv)

diff --git a/dynamixel_workbench_toolbox/CMakeLists.txt b/dynamixel_workbench_toolbox/CMakeLists.txt
@@ -4,8 +4,8 @@
 cmake_minimum_required(VERSION 3.0.2)
 project(dynamixel_workbench_toolbox)
 
-## Compile as C++11, supported in ROS Kinetic and newer
-add_compile_options(-std=c++11)
+## Compile as C++14, supported in ROS Melodic and newer
+add_compile_options(-std=c++14)
 
 ################################################################################
 # Find catkin packages and libraries for catkin and system dependencies

diff --git a/dynamixel_workbench_toolbox/examples/CMakeLists.txt b/dynamixel_workbench_toolbox/examples/CMakeLists.txt
@@ -1,7 +1,7 @@
 cmake_minimum_required(VERSION 3.0.2)
 project(dynamixel_workbench)
 
-add_compile_options(-std=c++11)
+add_compile_options(-std=c++14)
 
 include_directories(
   ../src

diff --git a/dynamixel_workbench_toolbox/include/dynamixel_workbench_toolbox/dynamixel_driver.h b/dynamixel_workbench_toolbox/include/dynamixel_workbench_toolbox/dynamixel_driver.h
@@ -27,6 +27,65 @@
 #elif defined(__linux__) || defined(__APPLE__)
   #include "unistd.h"
   #include "dynamixel_sdk/dynamixel_sdk.h"
+#elif defined(_WIN32)
+
+  #include <chrono>
+  #include <thread>
+
+  #include "dynamixel_sdk/dynamixel_sdk.h"
+
+  #ifndef _UNISTD_H
+  #define _UNISTD_H    1
+
+  /* This is intended as a drop-in replacement for unistd.h on Windows.
+  * Please add functionality as neeeded.
+  * https://stackoverflow.com/a/826027/1202830
+  */
+
+  #include <stdlib.h>
+  #include <io.h>
+  //#include <getopt.h> /* getopt at: https://gist.github.com/ashelly/7776712 */
+  #include <process.h> /* for getpid() and the exec..() family */
+  #include <direct.h> /* for _getcwd() and _chdir() */
+
+  #define srandom srand
+  #define random rand
+
+  /* Values for the second argument to access. These may be OR'd together.  */
+  #define R_OK    4       /* Test for read permission.  */
+  #define W_OK    2       /* Test for write permission.  */
+  //#define   X_OK    1       /* execute permission - unsupported in windows*/
+  #define F_OK    0       /* Test for existence.  */
+
+  #define access _access
+  #define dup2 _dup2
+  #define execve _execve
+  #define ftruncate _chsize
+  #define unlink _unlink
+  #define fileno _fileno
+  #define getcwd _getcwd
+  #define chdir _chdir
+  #define isatty _isatty
+  #define lseek _lseek
+  /* read, write, and close are NOT being #defined here, because while there are file handle specific versions for Windows, they probably don't work for sockets. You need to look at your app and consider whether to call e.g. closesocket(). */
+
+  #define ssize_t __int64
+
+  #define STDIN_FILENO 0
+  #define STDOUT_FILENO 1
+  #define STDERR_FILENO 2
+  /* should be in some equivalent to <sys/types.h> */
+  //typedef __int8            int8_t;
+  typedef __int16           int16_t; 
+  typedef __int32           int32_t;
+  typedef __int64           int64_t;
+  typedef unsigned __int8   uint8_t;
+  typedef unsigned __int16  uint16_t;
+  typedef unsigned __int32  uint32_t;
+  typedef unsigned __int64  uint64_t;
+
+  #endif /* unistd.h  */
+
 #endif
 
 #define MAX_DXL_SERIES_NUM  5

diff --git a/dynamixel_workbench_toolbox/include/dynamixel_workbench_toolbox/dynamixel_item.h b/dynamixel_workbench_toolbox/include/dynamixel_workbench_toolbox/dynamixel_item.h
@@ -46,6 +46,11 @@
 #define XL330_M077 1190
 #define XL330_M288 1200
 
+#define XC330_M181 1230
+#define XC330_M288 1240
+#define XC330_T181 1210
+#define XC330_T288 1220
+
 #define XL430_W250 1060
 
 #define XL430_W250_2 1090 // 2XL

diff --git a/dynamixel_workbench_toolbox/include/dynamixel_workbench_toolbox/dynamixel_workbench.h b/dynamixel_workbench_toolbox/include/dynamixel_workbench_toolbox/dynamixel_workbench.h
@@ -90,8 +90,11 @@ class DynamixelWorkbench : public DynamixelDriver
   float convertValue2Velocity(uint8_t id, int32_t value);
 
   int16_t convertCurrent2Value(uint8_t id, float current);
-  int16_t convertCurrent2Value(float current);
   float convertValue2Current(uint8_t id, int16_t value);
+
+  // This function will return incorrect value for some DYNAMIXEL like P or X330 series
+  int16_t convertCurrent2Value(float current);
+  // This function will return incorrect value for some DYNAMIXEL like P or X330 series
   float convertValue2Current(int16_t value);
 
   float convertValue2Load(int16_t value);