finished develop

CMakeLists.txt

@@ -0,0 +1,48 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(g29_ff)
+
+## Compile as C++11, supported in ROS Kinetic and newer
+add_compile_options(-std=c++11)
+
+find_package(catkin REQUIRED COMPONENTS
+    roscpp
+    rospy
+    message_generation
+)
+
+add_message_files(
+    FILES
+    ForceFeedback.msg
+)
+
+generate_messages(
+    DEPENDENCIES
+    std_msgs
+)
+
+## Generate dynamic reconfigure parameters in the 'cfg' folder
+# generate_dynamic_reconfigure_options(
+#   cfg/DynReconf1.cfg
+#   cfg/DynReconf2.cfg
+# )
+
+catkin_package(
+    #  INCLUDE_DIRS include
+    #  LIBRARIES g29_ff
+    CATKIN_DEPENDS roscpp rospy
+    #  DEPENDS system_lib
+)
+
+include_directories(
+    # include
+    ${catkin_INCLUDE_DIRS}
+)
+
+## Declare a C++ library
+# add_library(${PROJECT_NAME} src/${PROJECT_NAME}/g29_ff.cpp)
+# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
+add_executable(${PROJECT_NAME}_node src/g29_ff.cpp)
+# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+target_link_libraries(${PROJECT_NAME}_node ${catkin_LIBRARIES})
+set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "")

msg/ForceFeedback.msg

@@ -0,0 +1,3 @@
+std_msgs/Header header
+float32 angle
+float32 force

package.xml

@@ -0,0 +1,68 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>g29_ff</name>
+  <version>0.0.0</version>
+  <description>The g29_ff package</description>
+
+  <!-- One maintainer tag required, multiple allowed, one person per tag -->
+  <!-- Example:  -->
+  <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
+  <maintainer email="kuriatsu@todo.todo">kuriatsu</maintainer>
+
+
+  <!-- One license tag required, multiple allowed, one license per tag -->
+  <!-- Commonly used license strings: -->
+  <!--   BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
+  <license>TODO</license>
+
+
+  <!-- Url tags are optional, but multiple are allowed, one per tag -->
+  <!-- Optional attribute type can be: website, bugtracker, or repository -->
+  <!-- Example: -->
+  <!-- <url type="website">http://wiki.ros.org/g29_ff</url> -->
+
+
+  <!-- Author tags are optional, multiple are allowed, one per tag -->
+  <!-- Authors do not have to be maintainers, but could be -->
+  <!-- Example: -->
+  <!-- <author email="jane.doe@example.com">Jane Doe</author> -->
+
+
+  <!-- The *depend tags are used to specify dependencies -->
+  <!-- Dependencies can be catkin packages or system dependencies -->
+  <!-- Examples: -->
+  <!-- Use depend as a shortcut for packages that are both build and exec dependencies -->
+  <!--   <depend>roscpp</depend> -->
+  <!--   Note that this is equivalent to the following: -->
+  <!--   <build_depend>roscpp</build_depend> -->
+  <!--   <exec_depend>roscpp</exec_depend> -->
+  <!-- Use build_depend for packages you need at compile time: -->
+  <!--   <build_depend>message_generation</build_depend> -->
+  <!-- Use build_export_depend for packages you need in order to build against this package: -->
+  <!--   <build_export_depend>message_generation</build_export_depend> -->
+  <!-- Use buildtool_depend for build tool packages: -->
+  <!--   <buildtool_depend>catkin</buildtool_depend> -->
+  <!-- Use exec_depend for packages you need at runtime: -->
+  <!--   <exec_depend>message_runtime</exec_depend> -->
+  <!-- Use test_depend for packages you need only for testing: -->
+  <!--   <test_depend>gtest</test_depend> -->
+  <!-- Use doc_depend for packages you need only for building documentation: -->
+  <!--   <doc_depend>doxygen</doc_depend> -->
+  <buildtool_depend>catkin</buildtool_depend>
+  <build_depend>roscpp</build_depend>
+  <build_depend>rospy</build_depend>
+  <build_depend>message_generation</build_depend>
+  <build_export_depend>roscpp</build_export_depend>
+  <build_export_depend>rospy</build_export_depend>
+  <build_export_depend>message_generation</build_export_depend>
+  <exec_depend>roscpp</exec_depend>
+  <exec_depend>rospy</exec_depend>
+  <exec_depend>message_generation</exec_depend>
+
+
+  <!-- The export tag contains other, unspecified, tags -->
+  <export>
+    <!-- Other tools can request additional information be placed here -->
+
+  </export>
+</package>

src/g29_ff.cpp

@@ -0,0 +1,270 @@
+#include <ros/ros.h>
+#include <linux/input.h>
+#include <sys/ioctl.h>
+#include <unistd.h>
+#include <fcntl.h>
+#include <math.h>
+
+#include "g29_ff/ForceFeedback.h"
+
+class G29ForceFeedback
+{
+private:
+    ros::Subscriber sub_target;
+    ros::Timer timer;
+
+    // variables from fouce feedback API
+    int m_device_handle;
+    int m_axis_code = ABS_X;
+    int m_axis_min;
+    int m_axis_max;
+    struct ff_effect m_effect;
+
+    // device config
+    std::string m_device_name;
+    float m_max_force;
+    float m_min_force;
+
+    // motion config 0:PID force, 1:constant force
+    int m_mode;
+    double m_Kp = 1;
+    double m_Ki = 0.0;
+    double m_Kd = 0.1;
+    double m_offset = 0.01;
+
+    // target and current state of the wheel
+    double m_target_angle;
+    double m_target_force;
+    double m_current_angle;
+
+public:
+    G29ForceFeedback();
+
+private:
+    void targetCallback(const g29_ff::ForceFeedback &in_target);
+    void timerCallback(const ros::TimerEvent&);
+    int testBit(int bit, unsigned char *array);
+    void initFfDevice();
+    void updateFfDevice();
+};
+
+
+G29ForceFeedback::G29ForceFeedback() : m_device_name("/dev/input/event19"), m_mode(0)
+{
+    ros::NodeHandle n;
+    sub_target = n.subscribe("/ff_target", 1, &G29ForceFeedback::targetCallback, this);
+
+    n.getParam("device_name", m_device_name);
+    n.getParam("mode", m_mode);
+    n.getParam("Kp", m_Kp);
+    n.getParam("Ki", m_Ki);
+    n.getParam("Kd", m_Kd);
+    n.getParam("offset", m_offset);
+
+    initFfDevice();
+
+    ros::Duration(1).sleep();
+    timer = n.createTimer(ros::Duration(0.1), &G29ForceFeedback::timerCallback, this);
+}
+
+
+void G29ForceFeedback::timerCallback(const ros::TimerEvent&)
+{
+    updateFfDevice();
+}
+
+
+void G29ForceFeedback::updateFfDevice()
+{
+    struct input_event event;
+    static float diff_i = 0.0, diff = 0.0;
+    double diff_d, force, buf;
+
+    // if you wanna use I control, let's avoid integral value exploding
+    // static int count = 0;
+    // count ++;
+    // if (force > 0.3 || count > 10)
+    // {
+        //     diff_i = 0.0;
+        //     count = 0;
+        // }
+
+    // calcurate values for PID control
+    buf = diff;
+    diff = m_target_angle - m_current_angle;
+    diff_i += diff;
+    diff_d = diff - buf;
+
+    switch (m_mode)
+    {
+        case (0):
+        {
+            force = fabs(m_Kp * diff + m_Ki * diff_i + m_Kd * diff_d) * ((diff > 0.0) ? 1.0 : -1.0);
+
+            // if wheel angle reached to the target
+            if (fabs(diff) < m_offset)
+            {
+                force = 0.0;
+                break;
+            }
+            else
+            {
+                // force less than 0.2 cannot turn the wheel
+                force = (force > 0.0) ? std::max(force, m_min_force) : std::min(force, -m_min_force);
+                // set max force for safety
+                force = (force > 0.0) ? std::min(force, m_target_force) : std::max(force, -m_target_force);
+            }
+            break;
+        }
+
+        case (1):
+        {
+            force = fabs(force) * ((diff > 0.0) ? 1.0 : -1.0);
+            break;
+        }
+    }
+
+    // for safety
+    force = (force > 0.0) ? std::min(force, m_max_force) : std::max(force, -m_max_force);
+
+    // start effect
+    m_effect.u.constant.level = (short)(force * 32767.0);
+    m_effect.direction = 0xC000;
+    m_effect.u.constant.envelope.attack_level = (short)(force * 32767.0);
+    m_effect.u.constant.envelope.fade_level = (short)(force * 32767.0);
+
+    if (ioctl(m_device_handle, EVIOCSFF, &m_effect) < 0)
+    {
+        std::cout << "failed to upload m_effect" << std::endl;
+    }
+
+    // get current state
+    while (read(m_device_handle, &event, sizeof(event)) == sizeof(event))
+    {
+        if (event.type == EV_ABS && event.code == m_axis_code)
+        {
+            m_current_angle = (event.value - (m_axis_max + m_axis_min) * 0.5) * 2 / (m_axis_max - m_axis_min);
+        }
+    }
+}
+
+
+void G29ForceFeedback::targetCallback(const g29_ff::ForceFeedback &in_target)
+{
+    m_target_angle = in_target.angle;
+    m_target_force = in_target.force;
+}
+
+
+void G29ForceFeedback::initFfDevice()
+{
+    // setup device
+    unsigned char key_bits[1+KEY_MAX/8/sizeof(unsigned char)];
+    unsigned char abs_bits[1+ABS_MAX/8/sizeof(unsigned char)];
+    unsigned char ff_bits[1+FF_MAX/8/sizeof(unsigned char)];
+
+    struct input_event event;
+    struct input_absinfo abs_info;
+
+    m_device_handle = open(m_device_name.c_str(), O_RDWR|O_NONBLOCK);
+    if (m_device_handle < 0)
+    {
+        std::cout << "ERROR: cannot open device : "<< m_device_name << std::endl;
+        exit(1);
+    }else{std::cout << "device opened" << std::endl;}
+
+    // which axes has the device?
+    memset(abs_bits, 0, sizeof(abs_bits));
+    if (ioctl(m_device_handle, EVIOCGBIT(EV_ABS, sizeof(abs_bits)), abs_bits) < 0)
+    {
+        std::cout << "ERROR: cannot get abs bits" << std::endl;
+        exit(1);
+    }
+
+    // get some information about force feedback
+    memset(ff_bits, 0, sizeof(ff_bits));
+    if (ioctl(m_device_handle, EVIOCGBIT(EV_FF, sizeof(ff_bits)), ff_bits) < 0)
+    {
+        std::cout << "ERROR: cannot get ff bits" << std::endl;
+        exit(1);
+    }
+
+    // get axis value range
+    if (ioctl(m_device_handle, EVIOCGABS(m_axis_code), &abs_info) < 0)
+    {
+        std::cout << "ERROR: cannot get axis range" << std::endl;
+        exit(1);
+    }
+    m_axis_max = abs_info.maximum;
+    m_axis_min = abs_info.minimum;
+    if (m_axis_min >= m_axis_max)
+    {
+        std::cout << "ERROR: axis range has bad value" << std::endl;
+        exit(1);
+    }
+
+    // check force feedback is supported?
+    if(!testBit(FF_CONSTANT, ff_bits))
+    {
+        std::cout << "ERROR: force feedback is not supported" << std::endl;
+        exit(1);
+    }else{std::cout << "force feedback supported" << std::endl;}
+
+    // auto centering off
+    memset(&event, 0, sizeof(event));
+    event.type = EV_FF;
+    event.code = FF_AUTOCENTER;
+    event.value = 0;
+    if (write(m_device_handle, &event, sizeof(event)) != sizeof(event))
+    {
+        std::cout << "failed to disable auto centering" << std::endl;
+        exit(1);
+    }
+
+    // initialize constant foce m_effect
+    memset(&m_effect, 0, sizeof(m_effect));
+    m_effect.type = FF_CONSTANT;
+    m_effect.id = -1;
+    m_effect.trigger.button = 0;
+    m_effect.trigger.interval = 0;
+    m_effect.replay.length = 0xffff;
+    m_effect.replay.delay = 0;
+    m_effect.u.constant.level = 0;
+    m_effect.direction = 0xC000;
+    m_effect.u.constant.envelope.attack_length = 0;
+    m_effect.u.constant.envelope.attack_level = 0;
+    m_effect.u.constant.envelope.fade_length = 0;
+    m_effect.u.constant.envelope.fade_level = 0;
+
+    if (ioctl(m_device_handle, EVIOCSFF, &m_effect) < 0)
+    {
+        std::cout << "failed to upload m_effect" << std::endl;
+        exit(1);
+    }
+
+    // start m_effect
+    memset(&event, 0, sizeof(event));
+    event.type = EV_FF;
+    event.code = m_effect.id;
+    event.value = 1;
+    if (write(m_device_handle, &event, sizeof(event)) != sizeof(event))
+    {
+        std::cout << "failed to start event" << std::endl;
+        exit(1);
+    }
+}
+
+
+int G29ForceFeedback::testBit(int bit, unsigned char *array)
+{
+    return ((array[bit / (sizeof(unsigned char) * 8)] >> (bit % (sizeof(unsigned char) * 8))) & 1);
+}
+
+
+int main(int argc, char **argv)
+{
+    ros::init(argc, argv, "g29_ff_node");
+    G29ForceFeedback g29_ff;
+    ros::spin();
+    return(0);
+}