Merge pull request #263 from project-march/feature/PM-414-make-new-controller-template

Feature/pm 414 make new controller template

Author: Olavhaasie

march_joint_inertia_controller/CMakeLists.txt

@@ -0,0 +1,33 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(march_joint_inertia_controller)
+
+add_compile_options(-std=c++14 -Wall -Wextra -Werror)
+
+find_package(catkin REQUIRED COMPONENTS
+  controller_interface
+  pluginlib
+  roscpp
+)
+
+catkin_package(
+  INCLUDE_DIRS include
+  LIBRARIES ${PROJECT_NAME}
+)
+
+include_directories(include SYSTEM ${catkin_INCLUDE_DIRS})
+
+add_library(${PROJECT_NAME} src/joint_inertia_controller.cpp)
+target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES})
+
+install(DIRECTORY include/${PROJECT_NAME}/
+        DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
+        )
+
+install(TARGETS ${PROJECT_NAME}
+        ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+        LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+        )
+
+install(FILES controller_plugins.xml
+        DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+        )

march_joint_inertia_controller/config/joint_inertia_controller.yaml

@@ -0,0 +1,12 @@
+march:
+  joint_inertia_controller:
+    type: joint_inertia_controller/InertiaController
+    joints:
+      - left_ankle
+      - left_hip_aa
+      - left_hip_fe
+      - left_knee
+      - right_ankle
+      - right_hip_aa
+      - right_hip_fe
+      - right_knee

march_joint_inertia_controller/controller_plugins.xml

@@ -0,0 +1,5 @@
+<library path="lib/libjoint_inertia_controller_lib">
+  <class name="joint_inertia_controller/InertiaController"
+         type="joint_inertia_controller::InertiaController"
+         base_class_type="controller_interface::ControllerBase" />
+</library>

march_joint_inertia_controller/include/march_joint_inertia_controller/joint_inertia_controller.h

@@ -0,0 +1,26 @@
+// Copyright 2020 Project March.
+
+#ifndef MARCH_HARDWARE_JOINT_INERTIA_CONTROLLER_H
+#define MARCH_HARDWARE_JOINT_INERTIA_CONTROLLER_H
+
+#include <controller_interface/controller.h>
+#include <hardware_interface/joint_command_interface.h>
+#include <pluginlib/class_list_macros.h>
+
+namespace joint_inertia_controller
+{
+class InertiaController : public controller_interface::Controller<hardware_interface::PositionJointInterface>
+{
+public:
+  bool init(hardware_interface::PositionJointInterface* hw, ros::NodeHandle& n);
+  void update(const ros::Time& time, const ros::Duration& period);
+  void starting(const ros::Time& time);
+  void stopping(const ros::Time& time);
+
+private:
+  hardware_interface::JointHandle joint_;
+  double init_pos_;
+};
+}  // namespace joint_inertia_controller
+
+#endif  // MARCH_HARDWARE_JOINT_INERTIA_CONTROLLER_H

march_joint_inertia_controller/package.xml

@@ -0,0 +1,20 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>march_joint_inertia_controller</name>
+  <version>0.0.0</version>
+  <description>ROS controller which can be customized to operate in different circumstances.  </description>
+
+  <maintainer email="software@projectmarch.nl">Project March</maintainer>
+
+  <license>TODO</license>
+
+  <buildtool_depend>catkin</buildtool_depend>
+  <depend>roscpp</depend>
+  <depend>controller_interface</depend>
+  <depend>pluginlib</depend>
+
+  <export>
+    <controller_interface plugin="${prefix}/controller_plugins.xml" />
+
+  </export>
+</package>

march_joint_inertia_controller/src/joint_inertia_controller.cpp

@@ -0,0 +1,34 @@
+// Copyright 2020 Project March.
+#include "march_joint_inertia_controller/joint_inertia_controller.h"
+#include <math.h>
+
+namespace joint_inertia_controller
+{
+bool InertiaController::init(hardware_interface::PositionJointInterface* hw, ros::NodeHandle& nh)
+{
+  std::string joint_name;
+  if (!nh.getParam("joint_name", joint_name))
+  {
+    ROS_ERROR("No joint_name specified");
+    return false;
+  }
+  joint_ = hw->getHandle(joint_name);
+  return true;
+}
+
+void InertiaController::starting(const ros::Time& /* time */)
+{
+  init_pos_ = joint_.getPosition();
+}
+
+void InertiaController::update(const ros::Time& /* time */, const ros::Duration& /* period */)
+{
+  double dpos = init_pos_ + 10 * sin(ros::Time::now().toSec());
+  double cpos = joint_.getPosition();
+  joint_.setCommand(-10 * (cpos - dpos));
+}
+void InertiaController::stopping(const ros::Time& /* time */)
+{
+}
+}  // namespace joint_inertia_controller
+PLUGINLIB_EXPORT_CLASS(joint_inertia_controller::InertiaController, controller_interface::ControllerBase);