Merge pull request #2 from berkeleyauv/sub_thrusters

Sub thrusters - not super done

.gitignore

@@ -12,6 +12,3 @@ __pycache__/
 
 # C extensions
 *.so
-
-# Artifact from adding repo folder to a catkin workspace in the docker
-CMakeLists.txt

Dockerfile

@@ -24,8 +24,8 @@ RUN /bin/bash -c 'source /opt/ros/melodic/setup.bash \
 
 # Install ROS package dependencies
 RUN apt-get update \
-    && apt-get install ros-melodic-image-pipeline -y # For image_view \
-    && rosmake image_view
+    && apt-get install ros-melodic-image-pipeline ros-melodic-rqt -y # For image_view \
+    && rosmake image_view rqt
 
 # Add setup files to bashrc
 RUN echo source /opt/ros/melodic/setup.bash >> /root/.bashrc \

Makefile

@@ -16,5 +16,14 @@ run: ## Run container
 	docker run -it --rm -p 6080:80 -v="/tmp/.ros/:/root/.ros/" \
 		-v=$$(pwd):/root/catkin_ws/src/ --name=${APP_NAME} ${APP_NAME}
 
+run-more: ## Run container
+	# Beware if you have less than --cpus cpus on your computer
+	# Access docker stuff at localhost:6080 (type into your browser)
+	# Volumes: -v directoryA:directoryB means the stuff stored in directoryB in the docker container
+	#				can also be seen from directoyA on your computer.
+	#				aka the pwd one shares the git repo between the docker container and your computer
+	docker run -it --rm -p 6080:80 -v="/tmp/.ros/:/root/.ros/" \
+		-v=$$(pwd):/root/catkin_ws/src/ --cpus=1.5 --name=${APP_NAME} ${APP_NAME}
+
 clean: ## Remove a container, running or not
 	docker rm $(APP_NAME) -f

descriptions/urab_sub_control/urab_sub_thruster_manager/CHANGELOG.rst

@@ -0,0 +1,96 @@
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+Changelog for package uuv_thruster_manager
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+0.6.1 (2018-08-03)
+------------------
+
+0.6.0 (2018-07-31)
+------------------
+
+0.5.13 (2018-07-24)
+-------------------
+
+0.5.12 (2018-07-23)
+-------------------
+
+0.5.11 (2018-07-21)
+-------------------
+
+0.5.10 (2018-07-10)
+-------------------
+
+0.5.9 (2018-07-09)
+------------------
+
+0.5.8 (2018-07-07)
+------------------
+
+0.5.7 (2018-07-06)
+------------------
+
+0.5.6 (2018-07-06)
+------------------
+
+0.5.5 (2018-07-05)
+------------------
+* RM Merge messages from the change log
+  Signed-off-by: Musa Morena Marcusso Manhaes <[email protected]>
+* UPDATE Catkin packages format to 2
+  Signed-off-by: Musa Morena Marcusso Manhaes <[email protected]>
+* Contributors: Musa Morena Marcusso Manhaes
+
+0.5.4 (2018-07-04)
+------------------
+* UPDATE Catkin packages format to 2
+  Signed-off-by: Musa Morena Marcusso Manhaes <[email protected]>
+* Contributors: Musa Morena Marcusso Manhaes
+
+0.5.3 (2018-07-04)
+------------------
+* ADD CHANGELOG files
+  Signed-off-by: Musa Morena Marcusso Manhaes <[email protected]>
+* Contributors: Musa Morena Marcusso Manhaes
+
+0.5.1 (2018-07-03)
+------------------
+* CHANGE Bump version to 0.5.2
+  Signed-off-by: Musa Morena Marcusso Manhaes <[email protected]>
+* FIX Input for thrust force information for compuation of command value
+  Signed-off-by: Musa Morena Marcusso Manhaes <[email protected]>
+* CHANGE Remove prints from thruster allocator
+  Signed-off-by: Musa Morena Marcusso Manhaes <[email protected]>
+* CHANGE Test for invalid position and rotation inputs
+  Signed-off-by: Musa Morena Marcusso Manhaes <[email protected]>
+* CHANGE Version
+* ADD Subscriber for thruster_manager that is frame_id dependent
+  Signed-off-by: Musa Morena Marcusso Manhaes <[email protected]>
+* FIX Catkin requirements for catkin_make and catkin build
+  Signed-off-by: Musa Morena Marcusso Manhaes <[email protected]>
+* CHANGE Package versions
+  Signed-off-by: Musa Morena Marcusso Manhaes <[email protected]>
+* Add message generation dependency
+  Signed-off-by: Will Baker <[email protected]>
+* FIX Typos and package version
+  Signed-off-by: Musa Morena Marcusso Manhaes <[email protected]>
+* FIX RexROV thruster manager configuration
+  Signed-off-by: Musa Morena Marcusso Manhaes <[email protected]>
+* FIX TF listener timeout before TF frames are updated
+  Signed-off-by: Musa Morena Marcusso Manhaes <[email protected]>
+* FIX Package dependencies for rosdep
+  Signed-off-by: Musa Morena Marcusso Manhaes <[email protected]>
+* remove tf listener where not required
+  Signed-off-by: Sebastian Scherer <[email protected]>
+* Increasing thruster manager timeout while waiting for the /tf messages for computation of TAM
+  Signed-off-by: Musa Morena Marcusso Manhães <[email protected]>
+* Fixing the entry for max_thrust in the thruster manager. Now a list of max_thrust for each thruster can also be given.
+  Signed-off-by: Musa Morena Marcusso Manhães <[email protected]>
+* Fixing the thruster manager to accept multiple thruster models.
+  Signed-off-by: Musa Morena Marcusso Manhães <[email protected]>
+* install subdirectories in modules via setup.py
+  Signed-off-by: Sebastian Scherer <[email protected]>
+* fix several files not being installed (can now source install/setup.bash)
+  Signed-off-by: Sebastian Scherer <[email protected]>
+* initial commit
+  Signed-off-by: Sebastian Scherer (CR/AEI) <[email protected]>
+* Contributors: Musa Morena Marcusso Manhaes, Musa Morena Marcusso Manhães, Sebastian Scherer, Sebastian Scherer (CR/AEI), Will Baker

descriptions/urab_sub_control/urab_sub_thruster_manager/CMakeLists.txt

@@ -0,0 +1,32 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(urab_sub_thruster_manager)
+
+find_package(catkin REQUIRED COMPONENTS
+  std_msgs 
+  message_generation)
+
+catkin_python_setup()
+
+## Generate services in the 'srv' folder
+add_service_files(
+  FILES
+  GetThrusterCurve.srv
+  ThrusterManagerInfo.srv
+  SetThrusterManagerConfig.srv
+  GetThrusterManagerConfig.srv
+)
+
+## Generate added messages and services with any dependencies listed here
+generate_messages(
+  DEPENDENCIES
+  std_msgs
+)
+
+catkin_package(CATKIN_DEPENDS message_runtime)
+
+catkin_install_python(PROGRAMS scripts/thruster_allocator.py
+                      DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION})
+
+install(DIRECTORY launch config
+        DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+        PATTERN "*~" EXCLUDE)

descriptions/urab_sub_control/urab_sub_thruster_manager/config/TAM.yaml

@@ -0,0 +1,13 @@
+tam:
+- [0.0, 0.0, 0.0, 0.0, 0.7071068613706477, 0.7071068613706477, -0.7071065406341925,
+  -0.7071065406341925]
+- [0.0, 0.0, 0.0, 0.0, 0.7071067010024383, -0.7071067010024383, 0.7071070217388206,
+  -0.7071070217388206]
+- [1.0000000000000002, 1.0000000000000002, 1.0000000000000002, 1.0000000000000002,
+  0.0, 0.0, 0.0, 0.0]
+- [0.12070000000000003, -0.12070000000000003, 0.12070000000000003, -0.12070000000000003,
+  0.0, 0.0, 0.0, 0.0]
+- [0.12070000000000003, 0.12070000000000003, -0.12070000000000003, -0.12070000000000003,
+  0.0, 0.0, 0.0, 0.0]
+- [0.0, 0.0, 0.0, 0.0, -0.289998633100225, 0.289998633100225, 0.28999863310021007,
+  -0.28999863310021007]

descriptions/urab_sub_control/urab_sub_thruster_manager/config/thruster_manager.yaml

@@ -0,0 +1,52 @@
+thruster_manager:
+  tf_prefix: urab_sub
+  base_link: base_link
+  thruster_topic_prefix: thrusters/
+  thruster_topic_suffix: /input
+  thruster_frame_base: thruster_
+  max_thrust: 51.4 # Nominal forward thrust in Newtons (back is about 40.2)
+  max_clamp: 16 # Max nominal volts. 20V is maximum
+  timeout: -1
+  update_rate: 50 
+
+  ##################################################
+  # Options to set the thruster models below
+  ##################################################
+
+  # 1) If all thrusters have the same model (as described in the vehicle's robot description)
+  #
+  # 1.1) If the conversion function set for the thruster plugins is the following:
+  # <conversion>
+  #   <type>Basic</type>
+  #   <rotorConstant>0.0</rotorConstant>
+  # </conversion>
+  # You can set the conversion function to be:
+  conversion_fcn: proportional
+  conversion_fcn_params:
+    # Not sure if this changes anything??
+    gain: 0.000004 # <r otorConstant> T200, 12V for a 2nd order curve fit: tau = gain * abs(x)*x
+
+  # 1.2)  If the conversion function set for the thruster plugins is the following:
+  # <conversion>
+  #   <type>LinearInterp</type>
+  #   <inputValues>0 1 2 3</inputValues>
+  #   <outputValues>0 1 2 3</outputValues>
+  # </conversion>
+  # You can set the conversion function to be:
+  # conversion_fcn: custom
+  # conversion_fcn_params:
+  #   input: [0, 1, 2, 3]
+  #   output: [0, 1, 2, 3]
+
+  # 2) If the vehicle has thrusters with different models, you can list the
+  #    models descriptions. Beware to set the list where the index in list
+  #    matches the thruster ID
+  # conversion_fcn:
+  #   - proportional
+  #   - proportional
+  #   - custom
+  # conversion_fcn_params:
+  #   - gain: rotorConstant0
+  #   - gain: rotorConstant1
+  #   - input: [0, 1, 2, 3]
+#     output: [0, 1, 2, 3]

descriptions/urab_sub_control/urab_sub_thruster_manager/launch/thruster_manager.launch

@@ -0,0 +1,75 @@
+<?xml version="1.0"?>
+<launch>
+  <!-- Name of the robot model  -->
+  <arg name="model_name" default="urab_sub"/>
+  <!-- Namespace of the robot in the simulation (it can be different from the original model name) -->
+  <arg name="uuv_name" default="$(arg model_name)"/>
+  <!-- Name of the base link -->
+  <arg name="base_link" default="base_link" />
+  <!--
+    Thruster manager timeout.
+    If no wrench topic is received in the thruster
+    manager input for the time determined by the timeout, the manager will
+    publish zeros.
+    If the timeout is set to -1, the latest received input is always going to
+    be redistributed to the thruster units.
+  -->
+  <arg name="timeout" default="-1" />
+  <!--
+    In case the TAM.yaml must be generated or updated, you can set this flag to
+    true. The thruster allocation matrix is stored in a file because the
+    tf listener, that retrieves the pose of the thrusters wrt the vehicle's
+    base link in runtime, takes a long time to start. This initialization time
+    can make the vehicle's inactive until the tf frames are available. Be
+    sure to keep the thruster allocation matrix stored in a TAM.yaml to
+    improve the initialization of the thruster manager.
+    To generate the TAM.yaml, start the simulation with the vehicle and run:
+
+    >> roslaunch urab_sub_control start_thruster_manager.launch reset_tam:=true
+
+    The TAM.yaml file will be store in the folder defined by the following
+    argument output_dir
+  -->
+  <arg name="reset_tam" default="false"/>
+  <!-- Output folder to store the TAM.yaml and thruster_manager.yaml files -->
+  <arg name="output_dir" default="$(find urab_sub_thruster_manager)/config"/>
+  <!-- Thruster manager configuration file path -->
+  <arg name="config_file" default="$(find urab_sub_thruster_manager)/config/thruster_manager.yaml"/>
+  <!-- Thruster allocation matrix file path -->
+  <arg name="tam_file" default="$(find urab_sub_thruster_manager)/config/TAM.yaml"/>
+
+  <group ns="$(arg uuv_name)">
+    <rosparam file="$(arg config_file)" command="load"/>
+
+    <rosparam param="thruster_manager/tf_prefix" subst_value="true">
+      $(arg uuv_name)
+    </rosparam>
+
+    <rosparam param="thruster_manager/timeout" subst_value="true">
+      $(arg timeout)
+    </rosparam>
+
+    <group if="$(arg reset_tam)">
+      <node pkg="urab_sub_thruster_manager"
+            type="thruster_allocator.py"
+            name="thruster_allocator"
+            output="screen">
+        <!-- Output directory to store the thruster allocation matrix -->
+        <rosparam param="output_dir" subst_value="true">
+          $(arg output_dir)
+        </rosparam>
+      </node>
+    </group>
+
+    <group unless="$(arg reset_tam)">
+      <node pkg="urab_sub_thruster_manager"
+            type="thruster_allocator.py"
+            name="thruster_allocator"
+            output="screen">
+        <!-- Load thruster allocation matrix -->
+        <rosparam file="$(arg tam_file)" command="load"/>
+      </node>
+    </group>
+  </group>
+
+</launch>

descriptions/urab_sub_control/urab_sub_thruster_manager/package.xml

@@ -0,0 +1,28 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>urab_sub_thruster_manager</name>
+  <version>0.6.1</version>
+  <description>The thruster manager package</description>
+
+  <maintainer email="[email protected]">Musa Morena Marcusso Manhaes</maintainer>
+  <maintainer email="[email protected]">Sebastian Scherer</maintainer>
+  <maintainer email="[email protected]">Luiz Ricardo Douat</maintainer>
+
+  <author email="[email protected]">Musa Morena Marcusso Manhaes</author>
+  <author email="[email protected]">Sebastian Scherer</author>
+  <author email="[email protected]">Luiz Ricardo Douat</author>
+
+  <license>Apache-2.0</license>
+
+  <buildtool_depend>catkin</buildtool_depend>
+  <build_depend>message_generation</build_depend>
+
+  <exec_depend>std_msgs</exec_depend>
+  <exec_depend>message_runtime</exec_depend>
+  <exec_depend>tf</exec_depend>
+  <exec_depend>rospy</exec_depend>
+  <exec_depend>python-yaml</exec_depend>
+  <exec_depend>uuv_gazebo_ros_plugins_msgs</exec_depend>
+  <exec_depend>geometry_msgs</exec_depend>
+
+</package>