Add param handler and simulation for sensor (#11)

Added simulation support for the sensor. The data are inputed using joystick.

.travis.rosinstall

@@ -2,7 +2,6 @@
     uri: 'https://github.com/iirob/iirob_filters.git'
     local-name: iirob_filters
     version: kinetic-devel
-
 - git:
     uri: 'https://github.com/ipa320/cob_driver.git'
     local-name: cob_driver

.travis.yml

@@ -10,11 +10,10 @@ notifications:
 env:
   global:
     - UPSTREAM_WORKSPACE=file
-   # - VERBOSE_OUTPUT=true
+    # - VERBOSE_OUTPUT=true
   matrix:
     - ROS_DISTRO="kinetic" ROS_REPOSITORY_PATH=http://packages.ros.org/ros/ubuntu
 install:
   - git clone https://github.com/ros-industrial/industrial_ci.git .ci_config
 script:
   - source .ci_config/travis.sh
-#  - source ./travis.sh  # Enable this when you have a package-local script

CMakeLists.txt

@@ -4,14 +4,18 @@ project(ati_force_torque)
 find_package(catkin REQUIRED COMPONENTS
   cmake_modules
   cob_generic_can
+  dynamic_reconfigure 
+  gazebo_ros
   geometry_msgs
   iirob_filters
   message_generation
   roscpp
   roslaunch
+  rosparam_handler
   rospy
   std_msgs
   std_srvs
+  teleop_twist_joy
   tf2
   tf2_geometry_msgs
   tf2_ros
@@ -29,6 +33,21 @@ else()
   set(Eigen_LIBRARIES ${EIGEN3_LIBRARIES})
 endif()
 
+include(CheckCXXCompilerFlag)
+CHECK_CXX_COMPILER_FLAG("-std=c++11" Cpp11CompilerFlag)
+if (Cpp11CompilerFlag)
+	set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11")
+endif()
+
+generate_ros_parameter_files(
+    cfg/CoordinateSystemCalibration.params
+    cfg/CanConfiguration.params
+    cfg/FTSConfiguration.params
+    cfg/NodeConfiguration.params
+    cfg/PublishConfiguration.params
+    cfg/Calibration.params
+)
+
 add_service_files(
   FILES
   CalculateAverageMasurement.srv
@@ -46,26 +65,29 @@ generate_messages(
 ## catkin specific configuration ##
 ###################################
 catkin_package(
-  INCLUDE_DIRS common/include ros/include
-  CATKIN_DEPENDS roscpp rospy message_runtime std_msgs std_srvs cob_generic_can geometry_msgs tf2  tf2_ros tf2_geometry_msgs trajectory_msgs iirob_filters
-  DEPENDS Eigen
-  LIBRARIES ${PROJECT_NAME}
+ INCLUDE_DIRS common/include ros/include
+ CATKIN_DEPENDS cob_generic_can geometry_msgs iirob_filters message_runtime roscpp std_msgs std_srvs rospy tf2 tf2_ros tf2_geometry_msgs trajectory_msgs
+ DEPENDS Eigen
+ LIBRARIES ${PROJECT_NAME}
 )
 
 ###########
 ## Build ##
 ###########
 
 include_directories(
-  include
   common/include
   ros/include
   ${catkin_INCLUDE_DIRS}
   ${Eigen_INCLUDE_DIRS}
 )
 
-add_library(${PROJECT_NAME} common/src/ForceTorqueCtrl.cpp ros/src/force_torque_sensor.cpp ros/src/force_torque_sensor_handle.cpp)
-add_dependencies(${PROJECT_NAME} ${PROJECT_NAME}_generate_messages_cpp)
+add_library(${PROJECT_NAME} common/src/ForceTorqueCtrl.cpp ros/src/force_torque_sensor.cpp ros/src/force_torque_sensor_sim.cpp ros/src/force_torque_sensor_handle.cpp ros/src/force_torque_sensor_handle_sim.cpp)
+add_dependencies(${PROJECT_NAME}
+  ${PROJECT_NAME}_gencfg  # For dynamic_reconfigure
+  ${PROJECT_NAME}_generate_messages_cpp
+  ${PROJECT_NAME}_genparam # For rosparam_handler
+  )
 target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES} ${Eigen_LIBRARIES})
 
 add_executable(${PROJECT_NAME}_node ros/src/force_torque_sensor_node.cpp)
@@ -76,16 +98,9 @@ add_executable(${PROJECT_NAME}_config ros/src/force_torque_sensor_config.cpp)
 target_link_libraries(${PROJECT_NAME}_config ${PROJECT_NAME} ${catkin_LIBRARIES} ${Eigen_LIBRARIES})
 add_dependencies(${PROJECT_NAME}_config ${PROJECT_NAME} ${catkin_LIBRARIES} ${Eigen_LIBRARIES} ${PROJECT_NAME}_generate_messages_cpp)
 
-#############
-## Tests ##
-#############
-
-roslaunch_add_file_check(ros/launch)
-
 #############
 ## Install ##
 #############
-
 install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_node ${PROJECT_NAME}_config
   ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
   LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
@@ -94,8 +109,16 @@ install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_node ${PROJECT_NAME}_config
 
 install(DIRECTORY common/include ros/include
   DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
+  FILES_MATCHING PATTERN "*.h"
+  PATTERN ".svn" EXCLUDE
 )
 
 install(DIRECTORY ros/launch config description
   DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
 )
+
+#############
+## Tests ##
+#############
+
+roslaunch_add_file_check(ros/launch)

cfg/Calibration.params

@@ -0,0 +1,11 @@
+#!/usr/bin/env python
+from rosparam_handler.parameter_generator_catkin import *
+gen = ParameterGenerator()
+
+gen.add("n_measurements", paramtype="int", description="number of necessary measurements for calibration", default=100, configurable=True)
+gen.add("T_between_meas", paramtype="double", description="time between two measurements for calibration", default=0.1, configurable=True)
+gen.add("isStatic", paramtype="bool", description="is static calibration active", default=False, configurable=True)
+gen.add("force", paramtype="std::map<std::string,double>", description="calibration_offset_vector_force", default={"x":0.0, "y":0.0, "z":0.0})
+gen.add("torque", paramtype="std::map<std::string,double>", description="calibration_offset_vector_torque",default={"x":0.0, "y":0.0, "z":0.0})
+
+exit(gen.generate("ati_force_torque", "ForceTorqueSensor", "Calibration"))

cfg/CanConfiguration.params

@@ -0,0 +1,9 @@
+#!/usr/bin/env python
+from rosparam_handler.parameter_generator_catkin import *
+gen = ParameterGenerator()
+
+gen.add("type", paramtype="int", description="socket_can", default=-1, configurable=False)
+gen.add("path", paramtype="std::string", description="socket_can", default=" ", configurable=False)
+gen.add("baudrate", paramtype="int", description="baudrate", default=-1, configurable=False)
+
+exit(gen.generate("ati_force_torque", "ForceTorqueSensor", "CanConfiguration"))

cfg/CoordinateSystemCalibration.params

@@ -0,0 +1,9 @@
+#!/usr/bin/env python
+from rosparam_handler.parameter_generator_catkin import *
+gen = ParameterGenerator()
+
+gen.add("n_measurements", paramtype="int", description="number of necessary measurements for calibration", default=0, configurable=True)
+gen.add("T_between_meas", paramtype="int", description="time between two measurements for calibration", default=0, configurable=True)
+gen.add("push_direction", paramtype="int", description="push direction", default=0, configurable=False)
+
+exit(gen.generate("ati_force_torque", "ForceTorqueSensor", "CoordinateSystemCalibration"))

cfg/FTSConfiguration.params

@@ -0,0 +1,9 @@
+#!/usr/bin/env python
+from rosparam_handler.parameter_generator_catkin import *
+gen = ParameterGenerator()
+
+gen.add("base_identifier", paramtype="int", description="identifier", default=0x00, configurable=False)
+gen.add("auto_init", paramtype="bool", description="auto init", default=True, configurable=False)
+gen.add("fts_name", paramtype="std::string", description="name of sensor", default="", configurable=False)
+
+exit(gen.generate("ati_force_torque", "ForceTorqueSensor", "FTSConfiguration"))

cfg/NodeConfiguration.params

@@ -0,0 +1,11 @@
+#!/usr/bin/env python
+from rosparam_handler.parameter_generator_catkin import *
+gen = ParameterGenerator()
+
+gen.add("sim", paramtype="bool", description="if simulation or not", default=False, configurable=False)
+gen.add("ft_pub_freq", paramtype="double", description="publish frequency", default=200, configurable=False)
+gen.add("ft_pull_freq", paramtype="double", description="sensor daa pull frequency", default=800, configurable=False)
+gen.add("transform_frame", paramtype="std::string", description="reference coordinate system", default="fts_transform_frame", configurable=False)
+gen.add("sensor_frame", paramtype="std::string", description="sensor coordinate system", default="fts_reference_link", configurable=False)
+
+exit(gen.generate("ati_force_torque", "ForceTorqueSensor", "NodeConfiguration"))

cfg/PublishConfiguration.params

@@ -0,0 +1,12 @@
+#!/usr/bin/env python
+from rosparam_handler.parameter_generator_catkin import *
+gen = ParameterGenerator()
+
+gen.add("sensor_data", paramtype="bool", description="publsih raw sensor data", default=True, configurable=True)
+gen.add("low_pass", paramtype="bool", description="publish low pass filtered data", default=True, configurable=True)
+gen.add("moving_mean", paramtype="bool", description="publish moving mean filtered data", default=True, configurable=True)
+gen.add("transformed_data", paramtype="bool", description="publish transformed data", default=True, configurable=True)
+gen.add("gravity_compensated", paramtype="bool", description="publish gravity compensated data", default=True, configurable=True)
+gen.add("threshold_filtered", paramtype="bool", description="publish threshold filtered data", default=True, configurable=True)
+
+exit(gen.generate("ati_force_torque", "ForceTorqueSensor", "PublishConfiguration"))

config/sensor_configuration.yaml

@@ -1,13 +1,15 @@
 Node:
+    sim: false
     ft_pub_freq: 200
     ft_pull_freq: 800
     sensor_frame: "fts_reference_link"
     transform_frame: "fts_base_link"
 
 Calibration:
-    n_measurements: 500
-    T_between_meas: 0.01
-    static: true # true= use static offset Calibration, false=use calibrate Sensor to 0 in current State
+    Offset:
+        n_measurements: 500
+        T_between_meas: 0.01
+        isStatic: false # true= use static offset Calibration, false=use calibrate Sensor to 0 in current State
 
 ThresholdFilter:
   linear_threshold: 2.5

config/sensor_configuration_robot.yaml

@@ -0,0 +1,73 @@
+Node:
+    sim: false
+    ft_pub_freq: 200
+    ft_pull_freq: 800
+    sensor_frame: "fts_reference_link"
+    transform_frame: "fts_transform_frame"
+
+Calibration:
+    Offset:
+        n_measurements: 500
+        T_between_meas: 0.01
+        isStatic: false # true= use static offset Calibration, false=use calibrate Sensor to 0 in current State
+
+ThresholdFilter:
+  linear_threshold: 2.5
+  angular_threshold: 0.3
+
+LowPassFilter:
+    Force_x:
+        SamplingFrequency: 200.0
+        DampingFrequency: 15.0
+        DampingIntensity: -6.0
+    Force_y:
+        SamplingFrequency: 200.0
+        DampingFrequency: 15.0
+        DampingIntensity: -6.0
+    Force_z:
+        SamplingFrequency: 200.0
+        DampingFrequency: 15.0
+        DampingIntensity: -6.0
+    Torque_x:
+        SamplingFrequency: 200.0
+        DampingFrequency: 15.0
+        DampingIntensity: -6.0
+    Torque_y:
+        SamplingFrequency: 200.0
+        DampingFrequency: 15.0
+        DampingIntensity: -6.0
+    Torque_z:
+        SamplingFrequency: 200.0
+        DampingFrequency: 15.0
+        DampingIntensity: -6.0
+
+MovingMeanFilter:
+    Force_x:
+        divider: 4
+    Force_y:
+        divider: 4
+    Force_z:
+        divider: 4
+    Torque_x:
+        divider: 4
+    Torque_y:
+        divider: 4
+    Torque_z:
+        divider: 4
+
+GravityCompensation:
+    #CoG:
+     #x: 0
+     #y: 0
+     #z: 0.027346102 # m
+    #force: 7.9459955 # f_G
+    sensor_frame: "fts_reference_link"
+    world_frame: "fts_transform_frame"
+
+Publish:
+    sensor_data: true
+    low_pass: true
+    moving_mean: true
+    transformed_data: true
+    gravity_compensated: true
+    threshold_filtered: true

config/sensor_configuration_sim.yaml

@@ -0,0 +1,18 @@
+Node:
+    sim: true
+    ft_pub_freq: 200
+    ft_pull_freq: 800
+    sensor_frame: ""
+    transform_frame: ""
+    sim_forceX_param: 40
+    sim_forceY_param: 55
+    sim_torqueZ_param: 10
+    sim: true
+
+Publish:
+    sensor_data: true
+    low_pass: true
+    moving_mean: true
+    transformed_data: true
+    gravity_compensated: true
+    threshold_filtered: true

config/sensor_offset.yaml

@@ -1,2 +1,3 @@
 force: {x: -19.104102666356425, y: -1.5473841984049037, z: 0.9332121238751951}
 torque: {x: -0.24642145895240927, y: 0.0373995095506341, z: 0.060428603494093}
+

config/socket_can_ati.yaml

@@ -5,4 +5,4 @@ CAN:
 FTS:
     base_identifier: 0x20
     auto_init: true
-    name: "ATI_45_Mini"
+    fts_name: "ATI_45_Mini"

config/teleop_sim.yaml

@@ -0,0 +1,9 @@
+axis_linear: { x: 1, y: 0, z: 4 }  # Left thumb stick 
+scale_linear: { x: 12.0, y: 12.0, z: 12.0 } 
+
+axis_angular: { roll: 4, pitch: 3, yaw: 2 }  # Right thumb stick 
+scale_angular: { roll: 12.0, pitch: 12.0, yaw: 12.0 } 
+
+enable_button: 5
+
+

description/urdf/fts.gazebo.xacro

@@ -0,0 +1,25 @@
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
+
+  <xacro:macro name="fts_gazebo_ref_link" params="parent ">
+    <gazebo reference="${parent}_reference_link">
+      <gravity>true</gravity>
+      <self_collide>false</self_collide>
+    </gazebo>
+  </xacro:macro>
+
+    <xacro:macro name="fts_gazebo_base_link" params="parent ">
+    <gazebo reference="${parent}_base_link">
+      <gravity>true</gravity>
+      <self_collide>false</self_collide>
+    </gazebo>
+  </xacro:macro>
+
+    <xacro:macro name="fts_gazebo_tool_link" params="parent ">
+    <gazebo reference="${parent}_tool_link">
+      <gravity>true</gravity>
+      <self_collide>false</self_collide>
+    </gazebo>
+  </xacro:macro>
+
+</robot>

description/urdf/fts.transmission.xacro

@@ -0,0 +1,18 @@
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
+
+  <!--  FTS transmission   -->
+  <xacro:macro name="fts_transmission" params="parent">
+
+    <transmission name="${parent}_fts_trans">
+      <type>transmission_interface/SimpleTransmission</type>
+      <joint name="${parent}_reference_joint" >
+        <hardwareInterface>ForceTorqueSensorInterface</hardwareInterface>
+      </joint>
+        <actuator name="${parent}_actuator"> 
+            <hardwareInterface>ForceTorqueSensorInterface</hardwareInterface>
+      </actuator>
+    </transmission>
+  </xacro:macro>
+
+</robot>