Try to add force torque sensor controller and visualization of force and torque

Author: LucaGemma87

config/force_torque_sensor_control.yaml

@@ -0,0 +1,3 @@
+force_torque_sensor_controller:
+  type: force_torque_sensor_controller/ForceTorqueSensorController
+  publish_rate: 1000

launch/display_simulation.launch

@@ -8,7 +8,7 @@
   <node name="robot_tf_publisher" pkg="robot_state_publisher" type="robot_state_publisher"/>
 
   <!-- EXAMPLE FOR NANO17 -->
-      <node name="spawn_urdf" pkg="gazebo_ros" type="spawn_model" args="-param robot_description -urdf -model nano17"  respawn="false" output="screen" />
+      <node name="spawn_urdf" pkg="gazebo_ros" type="spawn_model" args="-param robot_description -urdf -model gamma"  respawn="false" output="screen" />
 
   <include file="$(find gazebo_ros)/launch/empty_world.launch">
       <arg name="world_name" value="$(find force_torque_sensor)/worlds/simple_environment.world"/>
@@ -21,4 +21,22 @@
 
   <node name="ft_sensor_rviz" pkg="rviz" type="rviz" respawn="false" args="-d $(find force_torque_sensor)/launch/my_config_rviz.rviz" output="screen"/>
 
+  <node name="rqt_plot_view_force" pkg="rqt_plot" type="rqt_plot" args="-e /world_my_sensor/ft_sensor_topic/wrench/force/x:y:z" />
+  <node name="rqt_plot_view_torque" pkg="rqt_plot" type="rqt_plot" args="-e /world_my_sensor/ft_sensor_topic/wrench/torque/x:y:z" />
+
+
+    <!-- Load joint controller configurations from YAML file to parameter server -->
+<!--   <rosparam file="$(find force_torque_sensor)/config/force_torque_sensor_control.yaml" command="load"/> -->
+   <rosparam command="load" file="$(find force_torque_sensor)/config/force_torque_sensor_control.yaml" />
+
+  <!-- load position controllers -->
+<!--   <node name="controller_spawner" pkg="controller_manager" type="spawner" respawn="false"
+    output="screen" ns="/ft_sensor" args="
+    joint_state_controller world_my_sensor_measure_joint"/>
+ -->
+
+    <node name="force_torque_sensor_controller" pkg="controller_manager" type="spawner" output="screen" args="force_torque_sensor_controller" /> 
+
+
+
 </launch>

launch/my_config_rviz.rviz

@@ -7,7 +7,6 @@ Panels:
         - /Global Options1
         - /Status1
         - /WrenchStamped1
-        - /WrenchStamped1/Status1
       Splitter Ratio: 0.5
     Tree Height: 595
   - Class: rviz/Selection
@@ -94,22 +93,22 @@ Visualization Manager:
         {}
       Update Interval: 0
       Value: false
-    - Alpha: 1
+    - Alpha: 0.5
       Arrow Width: 0.06
       Class: rviz/WrenchStamped
       Enabled: true
-      Force Arrow Scale: 2
+      Force Arrow Scale: 0.1
       Force Color: 255; 0; 0
       History Length: 1
       Name: WrenchStamped
       Topic: /world_my_sensor/ft_sensor_topic
-      Torque Arrow Scale: 2
+      Torque Arrow Scale: 0.1
       Torque Color: 0; 0; 127
       Value: true
   Enabled: true
   Global Options:
     Background Color: 48; 48; 48
-    Fixed Frame: world
+    Fixed Frame: my_sensor_gamma_measure
     Frame Rate: 30
   Name: root
   Tools:
@@ -130,7 +129,7 @@ Visualization Manager:
   Views:
     Current:
       Class: rviz/Orbit
-      Distance: 0.19719
+      Distance: 0.759513
       Enable Stereo Rendering:
         Stereo Eye Separation: 0.06
         Stereo Focal Distance: 1
@@ -142,10 +141,10 @@ Visualization Manager:
         Z: -0.00691727
       Name: Current View
       Near Clip Distance: 0.01
-      Pitch: 0.165397
+      Pitch: 0.0803973
       Target Frame: <Fixed Frame>
       Value: Orbit (rviz)
-      Yaw: 0.240414
+      Yaw: 3.71859
     Saved: ~
 Window Geometry:
   Displays:

model/ft_sensor.gazebo.xacro

@@ -3,6 +3,7 @@
 
 	<xacro:macro name="ft_sensor_gazebo" params="parent name type">
 
+
     <gazebo reference="${name}_${type}_base">
 			<gravity>true</gravity>
 		</gazebo>
@@ -24,7 +25,7 @@
         <noise>
           <type>gaussian</type>
           <mean>0.0</mean>
-          <stddev>0.003</stddev> <!-- change this to simulate noise --> 
+          <stddev>0.003</stddev> <!-- change this to simulate noise  0.003--> 
         </noise>
         </plugin>
     </gazebo>

model/ft_sensor.urdf.xacro

@@ -25,7 +25,7 @@
 
 			<parent link="${name}_${type}_base"/>
 			<child link="${name}_${type}_measure"/>
-			<limit lower="0" upper="0" effort="100" velocity="100"/>
+			<limit lower="0" upper="0" effort="0" velocity="0"/>
 			<axis xyz="0 0 1" />
 		</joint>