Changes in NUC on 25/06/19

uuv_control/uuv_control_cascaded_pids/config/anahita/inertial.yaml

@@ -1,9 +1,10 @@
 pid:
-  mass: 862.0
+  mass: 35
   inertial:
-    ixx: 243.39
+    ixx: 1.465
     ixy: 0
     ixz: 0
-    iyy: 367.20
+    iyy: 2.057
     iyz: 0
-    izz: 319.23
+    izz: 2.515
+

uuv_control/uuv_control_cascaded_pids/launch/joy_accel.launch

@@ -3,10 +3,10 @@
   <arg name="uuv_name" default="$(arg model_name)"/>
   <arg name="joy_id" default="0"/>
 
-  <include file="$(find uuv_thruster_manager)/launch/thruster_manager.launch">
+  <!--include file="$(find uuv_thruster_manager)/launch/thruster_manager.launch">
     <arg name="uuv_name" value="$(arg uuv_name)" />
     <arg name="model_name" value="$(arg model_name)" />
-  </include>
+  </include-->
 
   <group ns="$(arg uuv_name)">
     <rosparam file="$(find uuv_control_cascaded_pid)/config/$(arg model_name)/inertial.yaml" command="load"/>

uuv_control/uuv_control_cascaded_pids/launch/joy_velocity.launch

@@ -3,10 +3,10 @@
   <arg name="uuv_name" default="$(arg model_name)"/>
   <arg name="joy_id" default="0"/>
 
-  <include file="$(find uuv_thruster_manager)/launch/thruster_manager.launch">
+  <!--include file="$(find uuv_thruster_manager)/launch/thruster_manager.launch">
     <arg name="uuv_name" value="$(arg uuv_name)" />
     <arg name="model_name" value="$(arg model_name)" />
-  </include>
+  </include-->
 
   <group ns="$(arg uuv_name)">
     <rosparam file="$(find uuv_control_cascaded_pid)/config/$(arg model_name)/inertial.yaml" command="load"/>

uuv_control/uuv_trajectory_control/config/controllers/mb_sm/rexrov/params.yaml

@@ -0,0 +1,7 @@
+lambda: [10, 10, 10, 10, 10, 10]
+rho_constant: [10000, 10000, 10000, 10000, 10000, 10000]
+k: [500, 500, 500, 500, 500, 500]
+c : [50, 50, 50, 1, 1, 1]
+adapt_slope : [100, 10, 10]
+rho_0 : [3000, 3000, 8000, 1500, 1500, 8000]
+drift_prevent : 0.03

uuv_control/uuv_trajectory_control/config/controllers/nl_pid/rexrov/params.yaml

@@ -0,0 +1,4 @@
+Kp: [500, 500, 500, 300, 300, 300]
+Kd: [50, 50, 50, 20, 20, 20]
+Ki: [200, 200, 200, 100, 100, 100]
+Hm: [50, 50, 50, 40, 40, 40]

uuv_control/uuv_trajectory_control/config/controllers/nmb_sm/rexrov/params.yaml

@@ -0,0 +1,4 @@
+K: [5, 5, 5, 5, 5, 5]
+Kd: [1100, 1100, 1100, 1100, 1100, 1100]
+Ki: [0.09, 0.09, 0.09, 0.09, 0.09, 0.09]
+slope: [3, 3, 3, 3, 3, 3]

uuv_control/uuv_trajectory_control/config/controllers/pid/rexrov/params.yaml

@@ -0,0 +1,3 @@
+Kp: [3300, 3300, 3300, 3300, 3300, 330]
+Kd: [1100, 1100, 1100, 1100, 1100, 1100]
+Ki: [0, 0, 0, 0, 0, 0]

uuv_control/uuv_trajectory_control/config/models/rexrov/params.yaml

@@ -0,0 +1,26 @@
+mass: 1862.87
+model: rexrov
+inertial:
+  ixx: 525.39
+  iyy: 794.2
+  izz: 691.23
+  ixy: 1.44
+  ixz: 33.41
+  iyz: 2.6
+cog: [0, 0, 0]
+cob: [0, 0, 0.3]
+volume: 1.83826
+length: 2.6
+height: 1.6
+width: 1.5
+base_link: base_link
+Ma:
+- [700, 0, 0, 0, 0, 0]
+- [0, 1200, 0, 0, 0, 0]
+- [0, 0, 3500, 0, 0, 0]
+- [0, 0, 0, 500, 0, 0]
+- [0, 0, 0, 0, 800, 0]
+- [0, 0, 0, 0, 0, 200]
+linear_damping: [-70, -70, -700, -200, -300, -100]
+quad_damping: [-700, -900, -1800, -600, -700, -500]
+density: 1028.0

uuv_control/uuv_trajectory_control/launch/cascaded_pid_dp_controller.launch

@@ -23,19 +23,19 @@
   -->
 
   <!--Thruster manager configuration  -->
-  <arg name="thruster_manager_output_dir" default="$(find uuv_thruster_manager)/config/$(arg model_name)"/>
-  <arg name="thruster_manager_config_file" default="$(find uuv_thruster_manager)/config/$(arg model_name)/thruster_manager.yaml"/>
+  <!--arg name="thruster_manager_output_dir" default="$(find uuv_thruster_manager)/config/$(arg model_name)"/>
+  <arg name="thruster_manager_config_file" default="$(find uuv_thruster_manager)/config/$(arg model_name)/thruster_manager.yaml"/-->
   <!-- File containing the thruster allocation matrix -->
-  <arg name="tam_file" default="$(find uuv_thruster_manager)/config/$(arg model_name)/TAM.yaml"/>
+  <!--arg name="tam_file" default="$(find uuv_thruster_manager)/config/$(arg model_name)/TAM.yaml"/-->
 
   <!-- Start the thruster allocation manager -->
-  <include file="$(find uuv_thruster_manager)/launch/thruster_manager.launch">
+  <!--include file="$(find uuv_thruster_manager)/launch/thruster_manager.launch">
      <arg name="uuv_name" value="$(arg uuv_name)" />
      <arg name="model_name" value="$(arg model_name)" />
      <arg name="output_dir" value="$(arg thruster_manager_output_dir)" />
      <arg name="config_file" value="$(arg thruster_manager_config_file)" />
      <arg name="tam_file" value="$(arg tam_file)" />
-  </include>
+  </include-->
 
   <group ns="$(arg uuv_name)">
     <rosparam file="$(find uuv_control_cascaded_pid)/config/$(arg model_name)/inertial.yaml" command="load"/>

uuv_control/uuv_trajectory_control/src/uuv_control_interfaces/dp_controller_local_planner.py

@@ -266,7 +266,7 @@ def _apply_workspace_constraints(self, waypoint_set):
             inertial_frame_id=self.inertial_frame_id)
         for i in range(waypoint_set.num_waypoints):
             wp = waypoint_set.get_waypoint(i)
-            if wp.z > 0 and self.inertial_frame_id == 'world':
+            if wp.z > 0 and self.inertial_frame_id == 'odom':
                 continue
             if wp.z < 0 and self.inertial_frame_id == 'world_ned':
                 continue

uuv_teleop/launch/finned_uuv_teleop.launch

@@ -0,0 +1,86 @@
+<launch>
+<arg name="uuv_name" />
+<arg name="joy_id" default="0"/>
+<arg name="use_param_file" default="0"/>
+<arg name="filename" default="."/>
+
+<!-- Input arguments to map the joystick, default values for XBox 360 controller -->
+<arg name="axis_thruster" default="1"/>
+<arg name="axis_roll" default="0"/>
+<arg name="axis_pitch" default="4"/>
+<arg name="axis_yaw" default="3"/>
+
+<!--
+Arguments to describe the fin configuration, only used if no parameter
+file is available, for this default mode the thruster is modelled as
+  tau = rotorGain * angVel * abs(angVel)
+-->
+<arg name="thruster_rotor_gain" default="0.0009"/>
+<arg name="max_thrust" default="200"/>
+<arg name="thruster_topic" default="thrusters/0/input"/>
+<arg name="fin_topic_prefix" default="fins/"/>
+<arg name="fin_topic_suffix" default="/input"/>
+<arg name="thruster_joy_gain" default="1.0"/>
+<!--
+The default fin configuration considered here assumes that your fins are placed
+around the body of the vehicle as follows (on the YZ plane)
+Fin 0: (-y, z)
+Fin 1: (-y -z)
+Fin 2: (y, -z)
+Fin 3: (y, z)
+If you want to pass these lists on the command line, remove the empty spaces
+between the number
+-->
+<arg name="n_fins" default="4"/>
+<arg name="gain_roll" default="1,1,1,1"/>
+<arg name="gain_pitch" default="1,1,-1,-1"/>
+<arg name="gain_yaw" default="-1,1,1,-1"/>
+
+<!-- If option to use a parameter file is given -->
+<group if="$(arg use_param_file)">
+  <group ns="$(arg uuv_name)">
+    <node pkg="uuv_teleop" type="finned_uuv_teleop.py" name="finned_uuv_teleop"
+          output="screen">
+      <rosparam command="load" file="$(arg filename)"/>
+    </node>
+
+    <node pkg="joy" type="joy_node" name="joystick">
+      <param name="autorepeat_rate" value="10" />
+      <param name="dev" value="/dev/input/js$(arg joy_id)"/>
+    </node>
+  </group>
+</group>
+
+<group unless="$(arg use_param_file)">
+    <group ns="$(arg uuv_name)">
+      <node pkg="uuv_teleop" type="finned_uuv_teleop.py" name="finned_uuv_teleop"
+            output="screen">
+        <rosparam subst_value="true">
+          axis_thruster: $(arg axis_thruster)
+          axis_roll: $(arg axis_roll)
+          axis_pitch: $(arg axis_pitch)
+          axis_yaw: $(arg axis_yaw)
+          n_fins: $(arg n_fins)
+          thruster_joy_gain: $(arg thruster_joy_gain)
+          thruster_model:
+            name: proportional
+            max_thrust: $(arg max_thrust)
+            params:
+              gain: $(arg thruster_rotor_gain)
+          gain_roll: [$(arg gain_roll)]
+          gain_pitch: [$(arg gain_pitch)]
+          gain_yaw: [$(arg gain_yaw)]
+          thruster_topic: $(arg thruster_topic)
+          fin_topic_prefix: $(arg fin_topic_prefix)
+          fin_topic_suffix: $(arg fin_topic_suffix)
+        </rosparam>
+      </node>
+
+      <node pkg="joy" type="joy_node" name="joystick">
+        <param name="autorepeat_rate" value="10" />
+        <param name="dev" value="/dev/input/js$(arg joy_id)"/>
+      </node>
+    </group>
+  </group>
+
+</launch>

uuv_teleop/scripts/finned_uuv_teleop.py

@@ -0,0 +1,149 @@
+#!/usr/bin/env python
+# Copyright (c) 2016 The UUV Simulator Authors.
+# All rights reserved.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+import numpy
+import rospy
+import tf
+import tf.transformations as trans
+
+from sensor_msgs.msg import Joy
+from uuv_gazebo_ros_plugins_msgs.msg import FloatStamped
+from uuv_thrusters.models import Thruster
+from rospy.numpy_msg import numpy_msg
+
+
+class FinnedUUVControllerNode:
+    def __init__(self):
+        print('FinnedUUVControllerNode: initializing node')
+
+        self._ready = False
+
+        # Test if any of the needed parameters are missing
+        param_labels = ['n_fins', 'gain_roll', 'gain_pitch', 'gain_yaw',
+                        'thruster_model', 'fin_topic_prefix',
+                        'fin_topic_suffix', 'thruster_topic',
+                        'axis_thruster', 'axis_roll', 'axis_pitch', 'axis_yaw']
+
+        for label in param_labels:
+            if not rospy.has_param('~%s' % label):
+                raise rospy.ROSException('Parameter missing, label=%s' % label)
+
+        # Number of fins
+        self._n_fins = rospy.get_param('~n_fins')
+
+        # Thruster joy axis gain
+        self._thruster_joy_gain = 1
+        if rospy.has_param('~thruster_joy_gain'):
+            self._thruster_joy_gain = rospy.get_param('~thruster_joy_gain')
+
+        # Read the vector for contribution of each fin on the change on
+        # orientation
+        gain_roll = rospy.get_param('~gain_roll')
+        gain_pitch = rospy.get_param('~gain_pitch')
+        gain_yaw = rospy.get_param('~gain_yaw')
+
+        if len(gain_roll) != self._n_fins or len(gain_pitch) != self._n_fins \
+            or len(gain_yaw) != self._n_fins:
+            raise rospy.ROSException('Input gain vectors must have length '
+                                     'equal to the number of fins')
+
+        # Create the command angle to fin angle mapping
+        self._rpy_to_fins = numpy.vstack((gain_roll, gain_pitch, gain_yaw)).T
+
+        # Read the joystick mapping
+        self._joy_axis = dict(axis_thruster=rospy.get_param('~axis_thruster'),
+                              axis_roll=rospy.get_param('~axis_roll'),
+                              axis_pitch=rospy.get_param('~axis_pitch'),
+                              axis_yaw=rospy.get_param('~axis_yaw'))
+
+        # Subscribe to the fin angle topics
+        self._pub_cmd = list()
+        self._fin_topic_prefix = rospy.get_param('~fin_topic_prefix')
+        self._fin_topic_suffix = rospy.get_param('~fin_topic_suffix')
+        for i in range(self._n_fins):
+            topic = self._fin_topic_prefix + str(i) + self._fin_topic_suffix
+            self._pub_cmd.append(
+              rospy.Publisher(topic, FloatStamped, queue_size=10))
+
+        # Create the thruster model object
+        try:
+            self._thruster_topic = rospy.get_param('~thruster_topic')
+            self._thruster_params = rospy.get_param('~thruster_model')
+            if 'max_thrust' not in self._thruster_params:
+                raise rospy.ROSException('No limit to thruster output was given')
+            self._thruster_model = Thruster.create_thruster(
+                        self._thruster_params['name'], 0,
+                        self._thruster_topic, None, None,
+                        **self._thruster_params['params'])
+        except:
+            raise rospy.ROSException('Thruster model could not be initialized')
+
+        # Subscribe to the joystick topic
+        self.sub_joy = rospy.Subscriber('joy', numpy_msg(Joy),
+                                        self.joy_callback)
+
+        self._ready = True
+
+    def joy_callback(self, msg):
+        """Handle callbacks with joystick state."""
+
+        if not self._ready:
+            return
+
+        try:
+            thrust = max(0, msg.axes[self._joy_axis['axis_thruster']]) * \
+                self._thruster_params['max_thrust'] * \
+                self._thruster_joy_gain
+
+            cmd_roll = msg.axes[self._joy_axis['axis_roll']]
+            if abs(cmd_roll) < 0.2:
+                cmd_roll = 0.0
+
+            cmd_pitch = msg.axes[self._joy_axis['axis_pitch']]
+            if abs(cmd_pitch) < 0.2:
+                cmd_pitch = 0.0
+
+            cmd_yaw = msg.axes[self._joy_axis['axis_yaw']]
+            if abs(cmd_yaw) < 0.2:
+                cmd_yaw = 0.0
+
+            rpy = numpy.array([cmd_roll, cmd_pitch, cmd_yaw])
+            fins = self._rpy_to_fins.dot(rpy)
+
+            self._thruster_model.publish_command(thrust)
+
+            for i in range(self._n_fins):
+                cmd = FloatStamped()
+                cmd.data = fins[i]
+                self._pub_cmd[i].publish(cmd)
+
+            if not self._ready:
+                return
+        except Exception, e:
+            print 'Error occurred while parsing joystick input, check if the joy_id corresponds to the joystick ' \
+                  'being used. message=%s' % str(e)
+
+
+if __name__ == '__main__':
+    print('starting FinnedUUVControllerNode.py')
+    rospy.init_node('finned_uuv_teleop')
+
+    try:
+        node = FinnedUUVControllerNode()
+        rospy.spin()
+    except rospy.ROSInterruptException:
+        print('caught exception')
+    print('exiting')