multiple motors control

CMakeLists.txt

@@ -0,0 +1 @@
+/opt/ros/noetic/share/catkin/cmake/toplevel.cmake

README.md

@@ -0,0 +1,20 @@
+# ELMO_MAXON_ROS
+This is a C++ library for elmo driver, providing a high-level interface for controlling elmo motor or maxon.
+## BUILD THE SOURCE 
+```shell
+mkdir src
+cd src
+git clone [email protected]:zouyuelin/elmo_control_ros.git
+cd ..
+catkin_make
+```
+## USAGE
+```shell
+sudo su
+source devel/setup.sh
+rosrun elmo_control elmo_control_ros path_to/Setup.yaml
+```
+Publish the topic /elmo_velocity or /elmo_torque
+
+## CONFIG
+You can also change the yaml file for matching your own motor's features.

any_node/LICENSE

@@ -0,0 +1,29 @@
+Copyright 2019, ANYbotics AG.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions
+are met:
+
+1. Redistributions of source code must retain the above copyright
+   notice, this list of conditions and the following disclaimer.
+
+2. Redistributions in binary form must reproduce the above copyright
+   notice, this list of conditions and the following disclaimer in
+   the documentation and/or other materials provided with the
+   distribution.
+
+3. Neither the name of the copyright holder nor the names of its
+   contributors may be used to endorse or promote products derived
+   from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+"AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
+A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
+LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
+DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
+THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

any_node/README.md

@@ -0,0 +1,41 @@
+# ANY Node
+
+## Overview
+
+Set of wrapper packages to handle multi-threaded ROS nodes.
+
+This is research code, expect that it changes often and any fitness for a particular purpose is disclaimed.
+
+The source code is released under a [BSD 3-Clause license](LICENSE).
+
+## Building
+
+In order to install, clone the latest version from this repository into your catkin workspace and compile the packages.
+
+## Usage
+
+Please report bugs and request features using the [Issue Tracker](https://bitbucket.org/leggedrobotics/any_node/issues).
+
+## Packages
+
+This is only an overview. For more detailed documentation, please check the packages individually.
+
+
+### any_node
+
+ROS node wrapper with some convenience functions using *any_worker*.
+
+### any_worker
+
+High resolution and threaded version of the ROS rate class.
+
+### param_io
+
+Wrapper for the ROS param get and set functions outputting warnings if a parameter has not been found.
+
+### signal_handler
+
+Contains a static signal handling helper class.
+
+
+

any_node/any_node/CMakeLists.txt

@@ -0,0 +1,86 @@
+cmake_minimum_required(VERSION 3.5.1)
+project(any_node)
+
+find_package(catkin REQUIRED
+  COMPONENTS
+    any_worker
+    message_logger
+    param_io
+    roscpp
+    signal_handler
+)
+
+###################################
+## catkin specific configuration ##
+###################################
+catkin_package(
+  INCLUDE_DIRS
+    include
+  LIBRARIES
+    ${PROJECT_NAME}
+    pthread
+  CATKIN_DEPENDS
+    any_worker
+    message_logger
+    param_io
+    roscpp
+    signal_handler
+)
+
+###########
+## Build ##
+###########
+add_definitions(-std=c++11 -Wall)
+
+include_directories(
+  include
+  ${catkin_INCLUDE_DIRS}
+)
+
+
+add_library(${PROJECT_NAME}
+  src/Node.cpp
+)
+
+#############
+## Install ##
+#############
+install(DIRECTORY include/${PROJECT_NAME}/
+  DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
+  PATTERN ".svn" EXCLUDE
+)
+install(TARGETS ${PROJECT_NAME}
+  ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+)
+
+##########
+## Test ##
+##########
+set(CMAKE_EXPORT_COMPILE_COMMANDS ON)
+
+find_package(cmake_clang_tools QUIET)
+if(cmake_clang_tools_FOUND)
+  add_default_clang_tooling()
+endif(cmake_clang_tools_FOUND)
+
+
+if(CATKIN_ENABLE_TESTING)
+  catkin_add_gtest(test_${PROJECT_NAME}
+    test/EmptyTests.cpp
+    WORKING_DIRECTORY ${PROJECT_SOURCE_DIR}/test
+  )
+
+  if(TARGET test_${PROJECT_NAME})
+    target_link_libraries( test_${PROJECT_NAME}
+      ${PROJECT_NAME}
+      ${catkin_LIBRARIES}
+      gtest_main
+    )
+    find_package(cmake_code_coverage QUIET)
+    if(cmake_code_coverage_FOUND)
+      add_gtest_coverage()
+    endif(cmake_code_coverage_FOUND)
+  endif()
+endif()

any_node/any_node/README.md

@@ -0,0 +1,48 @@
+# Any_node (a.k.a. minimal nodewrap)
+
+## Overview
+Implements several convenience classes and functions.
+
+### Differences to ROS Nodes
+The any_node is a wrapper for the official [ROS node](http://wiki.ros.org/Nodes).
+It adds workers (high-precision version of ros::Rate class), custom signal handling and verbose ROS parameter reading, at the cost of less stable API.
+If these features are not explicitly required, it is recommended to use the official ROS node.
+
+### Param.hpp
+Forwards to param_io package, which allows to read and write ROS messages from and to the parameter server. Also, it provides param(..) functions which print a warning if a requested parameter was not found.
+
+### Topic.hpp
+Allows to advertise/subscribe to/from topics/services, whose connection details (topic name, latched, queue_size, deactivate, ...) are saved as ros parameters. Example yaml file:
+
+    publishers:
+      my_publisher_name:
+        topic: /my_publisher_topic_name
+        queue_size: 1
+        latch: false
+
+    subscribers:
+      my_subscriber_name:
+        topic: /my_subscriber_topic_name
+        queue_size: 1
+
+    servers:
+      my_service_server_name:
+        service: my_service_name
+
+    clients:
+      my_service_client_name:
+        service: my_service_name
+        persistent: false
+
+
+### Node.hpp
+Provides an interface base class any_node::Node, which declares init, cleanup and update functions and has a any_worker::WorkerManager instance.
+Classes derived from this are compatible with the Nodewrap template.
+Additionally, it forwards calls of subscribe, advertise, param, advertiseService serviceClient and addWorker calls to the above mentioned functions.
+See any_node_example for an example.
+
+### Nodewrap.hpp
+Convencience template, designed to be used with classes derived from any_node::Node.
+It automatically sets up ros nodehandlers (with private namespace) and spinners, signal handlers (like SIGINT, ...) and calls the init function on startup and cleanup on shutdown of the given Node.
+See any_node_example for an example.
+

any_node/any_node/include/any_node/Node.hpp

@@ -0,0 +1,167 @@
+/*!
+ * @file	Node.hpp
+ * @author	Philipp Leemann
+ * @date	July, 2016
+ */
+
+#pragma once
+
+#include <ros/ros.h>
+#include <sched.h>
+#include <unistd.h>  // for getpid()
+#include <memory>    // for std::shared_ptr
+
+#include "any_node/Param.hpp"
+#include "any_node/Topic.hpp"
+#include "any_worker/WorkerManager.hpp"
+#include "any_worker/WorkerOptions.hpp"
+
+namespace any_node {
+
+bool setProcessPriority(int priority);
+
+class Node {
+ public:
+  using NodeHandlePtr = std::shared_ptr<ros::NodeHandle>;
+
+  Node() = delete;
+  explicit Node(NodeHandlePtr nh);
+  virtual ~Node() = default;
+
+  /*
+   * (abstract) interface functions
+   */
+
+  /*!
+   * Init function, used to initialize all members and starting workers (if any).
+   * @return      True if successful. Returning false indicates that the node shall shut down.
+   */
+  virtual bool init() = 0;
+  /*!
+   * Pre-Cleanup function, which is called by Nodewrap _before_ stopping workers. (Thread safety up to the user!).
+   * This function is called even if init() returned false.
+   */
+  virtual void preCleanup() {}
+  /*!
+   * Cleanup function, called by Nodewrap _after_ stopping workers.
+   * This function is called even if init() returned false.
+   */
+  virtual void cleanup() = 0;
+
+  /*
+   * general
+   */
+
+  /*!
+   * Method to signal nodewrap to shutdown the node.
+   */
+  void shutdown();
+
+  /*!
+   * Helper functions to add Workers to the WorkerManager
+   */
+  template <class T>
+  inline bool addWorker(const std::string& name, const double timestep, bool (T::*fp)(const any_worker::WorkerEvent&), T* obj,
+                        const int priority = 0) {
+    return workerManager_.addWorker(name, timestep, fp, obj, priority);
+  }
+
+  inline bool addWorker(const any_worker::WorkerOptions& options) { return workerManager_.addWorker(options); }
+
+  /*!
+   * Check if WorkerManager is managing a Worker with given name
+   * @param name  Name of the worker
+   * @return      True if worker was found
+   */
+  inline bool hasWorker(const std::string& name) { return workerManager_.hasWorker(name); }
+
+  /*!
+   * Stop a worker managed by the WorkerManager
+   * @param name  Name of the worker
+   * @param wait  Whether to wait until the worker has finished or return immediately
+   */
+  inline void cancelWorker(const std::string& name, const bool wait = true) { workerManager_.cancelWorker(name, wait); }
+
+  /*!
+   * Method to stop all workers managed by the WorkerManager
+   */
+  inline void stopAllWorkers() { stopAllWorkers(true); }
+  inline void stopAllWorkers(bool wait) { workerManager_.cancelWorkers(wait); }
+
+  /*
+   * accessors
+   */
+  inline ros::NodeHandle& getNodeHandle() const { return *nh_; }
+
+  /*
+   * forwarding to Topic.hpp functions
+   */
+  template <typename msg>
+  inline ros::Publisher advertise(const std::string& name, const std::string& defaultTopic, uint32_t queue_size, bool latch = false) {
+    return any_node::advertise<msg>(*nh_, name, defaultTopic, queue_size, latch);
+  }
+
+  template <typename msg>
+  inline ThreadedPublisherPtr<msg> threadedAdvertise(const std::string& name, const std::string& defaultTopic, uint32_t queue_size,
+                                                     bool latch = false, unsigned int maxMessageBufferSize = 10) {
+    return any_node::threadedAdvertise<msg>(*nh_, name, defaultTopic, queue_size, latch, maxMessageBufferSize);
+  }
+
+  template <class M, class T>
+  inline ros::Subscriber subscribe(const std::string& name, const std::string& defaultTopic, uint32_t queue_size,
+                                   void (T::*fp)(const boost::shared_ptr<M const>&), T* obj,
+                                   const ros::TransportHints& transport_hints = ros::TransportHints()) {
+    return any_node::subscribe(*nh_, name, defaultTopic, queue_size, fp, obj, transport_hints);
+  }
+
+  template <class M, class T>
+  inline ThrottledSubscriberPtr<M, T> throttledSubscribe(double timeStep, const std::string& name, const std::string& defaultTopic,
+                                                         uint32_t queue_size, void (T::*fp)(const boost::shared_ptr<M const>&), T* obj,
+                                                         const ros::TransportHints& transport_hints = ros::TransportHints()) {
+    return any_node::throttledSubscribe<M, T>(timeStep, *nh_, name, defaultTopic, queue_size, fp, obj, transport_hints);
+  }
+
+  template <class T, class MReq, class MRes>
+  inline ros::ServiceServer advertiseService(const std::string& name, const std::string& defaultService, bool (T::*srv_func)(MReq&, MRes&),
+                                             T* obj) {
+    return any_node::advertiseService(*nh_, name, defaultService, srv_func, obj);
+  }
+
+  template <class MReq, class MRes>
+  inline ros::ServiceClient serviceClient(const std::string& name, const std::string& defaultService,
+                                          const ros::M_string& header_values = ros::M_string()) {
+    return any_node::serviceClient<MReq, MRes>(*nh_, name, defaultService, header_values);
+  }
+
+  template <class Service>
+  inline ros::ServiceClient serviceClient(const std::string& name, const std::string& defaultService,
+                                          const ros::M_string& header_values = ros::M_string()) {
+    return any_node::serviceClient<Service>(*nh_, name, defaultService, header_values);
+  }
+
+  /*
+   * forwarding to Param.hpp functions
+   */
+  template <typename ParamT>
+  inline bool getParam(const std::string& key, ParamT& param_val) {
+    return any_node::getParam(*nh_, key, param_val);
+  }
+
+  template <typename ParamT>
+  inline ParamT param(const std::string& key, const ParamT& defaultValue) {
+    return any_node::param(*nh_, key, defaultValue);
+  }
+
+  template <typename ParamT>
+  inline void setParam(const std::string& key, const ParamT& param) {
+    any_node::setParam(*nh_, key, param);
+  }
+
+ protected:
+  NodeHandlePtr nh_;
+
+ private:
+  any_worker::WorkerManager workerManager_;
+};
+
+}  // namespace any_node