Added joystick and keyboard (#8)

Author: destogl

config/joy.yaml

@@ -0,0 +1 @@
+dev: /dev/input/js0

config/sensor_configuration.yaml

@@ -1,13 +1,39 @@
 Node:
     sim: true
+    sensor_hw: "force_torque_sensor/ForceTorqueSensorSim"
     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
+
+Calibration:
+    Offset:
+        n_measurements: 50
+        T_between_meas: 0.01
+        isStatic: false # true= use static offset Calibration, false=use calibrate Sensor to 0 in current State
+
+ThresholdFilter:
+  name: ThresholdFilter
+  type: iirob_filters/ThresholdFilterWrench
+  params: {linear_threshold: 2.5,  angular_threshold: 0.3}
+
+LowPassFilter:
+  name: LowPassFilter
+  type: iirob_filters/LowPassFilterWrench
+  params: {SamplingFrequency: 200.0, DampingFrequency: 15.0, DampingIntensity: -6.0}
+
+MovingMeanFilter:
+  name: MovingMeanFilter
+  type: filters/MovingMeanFilterWrench
+  params: {divider: 4}
+
+GravityCompensation:
+  name: GravityCompensationWrench
+  type: iirob_filters/GravityCompensator
+  params: {world_frame: "base_link"}
 
 Publish:
     sensor_data: true

config/sensor_configuration_sim.yaml

@@ -1,2 +1,2 @@
-force: {x: -18.91429005844759, y: -2.2334844389400255, z: 3.609171777265083}
-torque: {x: -0.33333832840498523, y: 0.025680667375069872, z: 0.11678855859001577}
+force: {x: 0.0, y: 0.0, z: 0.0}
+torque: {x: 0.0, y: 0.0, z: 0.0}

config/sensor_offset.yaml

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

config/teleop_sim.yaml

@@ -0,0 +1,22 @@
+<?xml version="1.0"?>
+<launch>
+
+  <!-- publish joystick data -->
+  <node name="joy_pub" pkg="joy" type="joy_node">
+    <rosparam command="load" file="$(find force_torque_sensor)/config/joy.yaml"/>
+  </node>
+
+  <!-- format joystick data to force (published via /cmd_vel)-->
+  <node name="teleop" pkg="teleop_twist_joy" type="teleop_node">
+    <rosparam command="load" file="$(find force_torque_sensor)/config/teleop_sim.yaml"/>
+  </node>
+
+  <!-- publish data for simulated force torque sensor -->
+  <node name="fts" pkg="force_torque_sensor" type="force_torque_sensor_node" respawn="false" output="screen" >
+    <remap from="/cmd_force" to="/cmd_vel" />
+  </node>
+
+  <rosparam command="load" file="$(find force_torque_sensor)/config/sensor_configuration_sim.yaml"/>
+  <rosparam command="load" ns="Calibration/Offset" file="$(find force_torque_sensor)/config/sensor_offset.yaml" />
+
+</launch>

launch/fts_sim_joy.launch

@@ -0,0 +1,17 @@
+<?xml version="1.0"?>
+<launch>
+
+  <!-- format keyboard data to force (published via /cmd_vel)-->
+  <node name="teleop" pkg="teleop_twist_keyboard" type="teleop_twist_keyboard.py">
+    <rosparam command="load" file="$(find force_torque_sensor)/config/teleop_sim.yaml"/>
+  </node>
+
+  <!-- publish data for simulated force torque sensor -->
+  <node name="fts" pkg="force_torque_sensor" type="force_torque_sensor_node" respawn="false" output="screen" >
+    <remap from="/cmd_force" to="/cmd_vel" />
+  </node>
+
+  <rosparam command="load" file="$(find force_torque_sensor)/config/sensor_configuration_sim.yaml"/>
+  <rosparam command="load" ns="Calibration/Offset" file="$(find force_torque_sensor)/config/sensor_offset.yaml" />
+
+</launch>