scripts/float32_to_uuv_msgs.py

+#!/usr/bin/env python
+
+import rospy
+from std_msgs.msg import Float64
+from uuv_gazebo_ros_plugins_msgs.msg import FloatStamped
+
+
+rospy.init_node('republisher')
+in_topic = rospy.get_param('~in')
+out_topic = rospy.get_param('~out')
+
+pub = rospy.Publisher(out_topic, FloatStamped, queue_size=1)
+
+def msg_callback(msg):
+    uuv_msg = FloatStamped()
+    uuv_msg.data = msg.data
+    pub.publish(uuv_msg)
+
+rospy.Subscriber(in_topic, Float64, msg_callback)
+rospy.spin()

src/thrusters_controller_node.cpp

+#include <rclcpp/rclcpp.hpp>
+
+#include <eigen3/Eigen/Core>
+#include <eigen3/Eigen/Eigen>
+#include <eigen3/Eigen/SVD>
+
+#include <std_msgs/msg/float64.hpp>
+#include <uuv_gazebo_ros_plugins_msgs/msg/float_stamped.hpp>
+#include <geometry_msgs/msg/wrench.hpp>
+#include <geometry_msgs/msg/twist.hpp>
+
+using std::placeholders::_1;
+
+using Eigen::Vector3d;
+using Eigen::VectorXd;
+using Eigen::MatrixXd;
+typedef Eigen::Matrix<double,6,1> RowVector6d;
+typedef rclcpp::Publisher<uuv_gazebo_ros_plugins_msgs::msg::FloatStamped, std::allocator<void>> ThrusterPublisherT;
+
+class Thruster
+{
+public:
+    Thruster(std::shared_ptr<ThrusterPublisherT> pub, bool enabled, std::vector<double> &f, std::vector<double> &r, const std::vector<double> &input, const std::vector<double> &output)
+        : publisher(pub),
+          enabled(enabled)
+    {
+        assert(input.size() == output.size());
+        assert(f.size() == 3);
+        assert(r.size() == 3);
+
+        setf(f);
+        setr(r);
+        setLookupTable(input, output);
+    }
+
+    void publish(double cmd)
+    {
+        uuv_gazebo_ros_plugins_msgs::msg::FloatStamped msg;
+        msg.data = this->convert(cmd);
+        this->publisher->publish(msg);
+    }
+
+    void setEnabled(bool enabled) { this->enabled = enabled; }
+    void setf(std::vector<double> &f) { this->f = Vector3d(f[0], f[1], f[2]); }
+    void setr(std::vector<double> &r) { this->r = Vector3d(r[0], r[1], r[2]); }
+    void setLookupTable(const std::vector<double> &input, const std::vector<double> &output)
+    {
+        lookupTable.clear();
+        size_t len = std::min(input.size(), output.size());
+        for (size_t i = 0; i < len; i++)
+          lookupTable[input[i]] = output[i];
+    }
+
+    bool isEnabled() { return  enabled; }
+    Vector3d getf() { return f; }
+    Vector3d getr() { return r; }
+
+private:
+    double convert(double cmd)
+    {
+        assert(!lookupTable.empty());
+
+        // "first element whose key is NOT considered to go before cmd"
+        auto iter = lookupTable.lower_bound(cmd);
+
+        if (iter == lookupTable.end())
+        {
+          // "all keys are considered to go before"
+          // last element is closest
+          return lookupTable.rbegin()->second;
+        }
+
+        double i1 = iter->first;
+        double o1 = iter->second;
+
+        if (iter == lookupTable.begin())
+          return o1;
+
+        iter--;
+
+        double i0 = iter->first;
+        double o0 = iter->second;
+
+        double w1 = cmd - i0;
+        double w0 = i1 - cmd;
+
+        return (o0*w0 + o1*w1)/(w0 + w1);
+
+    }
+
+    std::shared_ptr<ThrusterPublisherT> publisher;
+
+    std::map<double, double> lookupTable;
+    bool enabled;
+    Vector3d f;
+    Vector3d r;
+};
+
+class ThrusterController : public rclcpp::Node
+{
+public:
+    ThrusterController()
+        : Node("thrusters_controller")
+    {
+        for (auto &o : this->get_node_parameters_interface()->get_parameter_overrides())
+        {
+            std::istringstream iss(o.first);
+            std::string token;
+            std::getline(iss, token, '.');
+            if (token == "thrusters")
+            {
+                std::getline(iss, token, '.');
+                thrusters_map_[token] = nullptr;
+            }
+        }
+
+        thrusters_vec_.reserve(thrusters_map_.size());
+        for (auto &t : thrusters_map_)
+        {
+            bool enabled = declare_parameter<bool>("thrusters." + t.first + ".enabled");
+
+            std::vector<double> f = declare_parameter<std::vector<double>>("thrusters." + t.first + ".f");
+            assert(f.size() == 3);
+
+            std::vector<double> r = declare_parameter<std::vector<double>>("thrusters." + t.first + ".r");
+            assert(r.size() == 3);
+
+            std::vector<double> input = declare_parameter<std::vector<double>>("thrusters." + t.first + ".input");
+            std::vector<double> output = declare_parameter<std::vector<double>>("thrusters." + t.first + ".sim_output");
+
+            auto pub = this->create_publisher<uuv_gazebo_ros_plugins_msgs::msg::FloatStamped>("thrusters/" + t.first, 1);
+
+            t.second = new Thruster(pub, enabled, f, r, input, output);
+
+            RCLCPP_INFO(this->get_logger(), "ADDED THRUSTER: %s", t.first.c_str());
+            RCLCPP_INFO_STREAM(this->get_logger(), "enabled: " << enabled << "; f: " << f[0] << " " << f[1] << " " << f[2] << "; r: " << r[0] << " " << r[1] << " " << r[2]);
+        }
+        calculateTam_();
+
+        wrench_subscription_ = this->create_subscription<geometry_msgs::msg::Wrench>(
+                    "wrench_command", 10, std::bind(&ThrusterController::wrench_callback, this, _1));
+        twist_subscription_ = this->create_subscription<geometry_msgs::msg::Twist>(
+                    "twist_command", 10, std::bind(&ThrusterController::twist_callback, this, _1));
+
+        callback_handle_ = this->add_on_set_parameters_callback(
+            std::bind(&ThrusterController::parametersCallback, this, std::placeholders::_1));
+    }
+
+    rcl_interfaces::msg::SetParametersResult parametersCallback(
+        const std::vector<rclcpp::Parameter> &parameters)
+    {
+        rcl_interfaces::msg::SetParametersResult result;
+        result.successful = true;
+        result.reason = "success";
+
+        for (auto &p : parameters)
+        {
+            RCLCPP_INFO(this->get_logger(), "Setting parameter: %s", p.get_name().c_str());
+
+            std::istringstream iss(p.get_name());
+            std::string parameter_name;
+            std::getline(iss, parameter_name, '.');
+            if (parameter_name == "thrusters")
+            {
+                std::string thruster_name;
+                std::getline(iss, thruster_name, '.');
+                if (Thruster *t = thrusters_map_[thruster_name])
+                {
+                    std::getline(iss, parameter_name, '.');
+                    if (parameter_name == "enabled")
+                    {
+                        t->setEnabled(p.get_parameter_value().get<bool>());
+                        calculateTam_();
+                    }
+                    else if (parameter_name == "f")
+                    {
+                        auto f = p.get_parameter_value().get<std::vector<double>>();
+                        if (f.size() != 3)
+                        {
+                            result.successful = false;
+                            result.reason = "f vector for thruster" + thruster_name + " size != 3";
+                            return result;
+                        }
+                        t->setf(f);
+                        calculateTam_();
+                    }
+                    else if (parameter_name == "r")
+                    {
+                        auto r = p.get_parameter_value().get<std::vector<double>>();
+                        if (r.size() != 3)
+                        {
+                            result.successful = false;
+                            result.reason = "r vector for thruster" + thruster_name + " size != 3";
+                            return result;
+                        }
+                        t->setr(r);
+                        calculateTam_();
+                    }
+                    else if (parameter_name == "input")
+                    {
+                        std::vector<double> input = p.get_parameter_value().get<std::vector<double>>();
+                        std::vector<double> output = get_parameter("thrusters." + thruster_name + ".output").get_value<std::vector<double>>();
+                        t->setLookupTable(input, output);
+
+                    }
+                    else if (parameter_name == "output")
+                    {
+                        std::vector<double> input = get_parameter("thrusters." + thruster_name + ".input").get_value<std::vector<double>>();
+                        std::vector<double> output = p.get_parameter_value().get<std::vector<double>>();
+                        t->setLookupTable(input, output);
+                    }
+                }
+            }
+        }
+
+        return result;
+    }
+
+private:
+    void wrench_callback(const geometry_msgs::msg::Wrench::SharedPtr msg)
+    {
+        RowVector6d wrench = readWrench_(msg);
+        VectorXd effort = tam_ * wrench;
+
+        for(unsigned i = 0; i < tam_.rows(); ++i)
+            thrusters_vec_[i]->publish(effort[i]);
+    }
+
+    void twist_callback(const geometry_msgs::msg::Twist::SharedPtr msg)
+    {
+        RowVector6d wrench = readTwist_(msg);
+        VectorXd effort = tam_ * wrench;
+
+        for(unsigned i = 0; i < tam_.rows(); ++i)
+            thrusters_vec_[i]->publish(effort[i]);
+    }
+
+    void calculateTam_()
+    {
+        thrusters_vec_.clear();
+        for (auto it = thrusters_map_.cbegin(); it != thrusters_map_.cend(); ++it)
+            thrusters_vec_.push_back(it->second);
+
+        MatrixXd T(6, thrusters_vec_.size());
+        RCLCPP_INFO_STREAM(this->get_logger(), "Calculating TAM; Thrusters count: " << thrusters_vec_.size());
+
+        for(size_t i = 0; i < thrusters_vec_.size(); ++i)
+        {
+            Vector3d f = thrusters_vec_[i]->isEnabled() ? thrusters_vec_[i]->getf() : Vector3d(0, 0, 0);
+            Vector3d r = thrusters_vec_[i]->getr();
+            T.col(i) << f, r.cross(f);
+        }
+
+        tam_ = pseudoInverse(T);
+
+        RCLCPP_INFO_STREAM(this->get_logger(), "T\n" << T);
+        RCLCPP_INFO_STREAM(this->get_logger(), "Inverse T:\n" << tam_);
+    }
+
+    RowVector6d readWrench_(const geometry_msgs::msg::Wrench::SharedPtr msg)
+    {
+        RowVector6d wrench;
+
+        wrench[0] = msg->force.x;
+        wrench[1] = msg->force.y;
+        wrench[2] = msg->force.z;
+        wrench[3] = msg->torque.x;
+        wrench[4] = msg->torque.y;
+        wrench[5] = msg->torque.z;
+
+        return wrench;
+    }
+
+    RowVector6d readTwist_(const geometry_msgs::msg::Twist::SharedPtr msg)
+    {
+        RowVector6d wrench;
+
+        wrench[0] = msg->linear.x;
+        wrench[1] = msg->linear.y;
+        wrench[2] = msg->linear.z;
+        wrench[3] = msg->angular.x;
+        wrench[4] = msg->angular.y;
+        wrench[5] = msg->angular.z;
+
+        return wrench;
+    }
+
+    template<typename _Matrix_Type_>
+    _Matrix_Type_ pseudoInverse(const _Matrix_Type_ &a, double epsilon = std::numeric_limits<double>::epsilon())
+    {
+        Eigen::JacobiSVD<_Matrix_Type_> svd(a, Eigen::ComputeThinU | Eigen::ComputeThinV);
+        double tolerance = epsilon * std::max(a.cols(), a.rows()) * svd.singularValues().array().abs()(0);
+        return svd.matrixV() * (svd.singularValues().array().abs() > tolerance).select(svd.singularValues().array().inverse(), 0).matrix().asDiagonal() * svd.matrixU().adjoint();
+    }
+
+    MatrixXd tam_;
+    std::vector<Thruster *> thrusters_vec_;
+    std::map<std::string, Thruster *> thrusters_map_;
+    rclcpp::Subscription<geometry_msgs::msg::Wrench>::SharedPtr wrench_subscription_;
+    rclcpp::Subscription<geometry_msgs::msg::Twist>::SharedPtr twist_subscription_;
+    OnSetParametersCallbackHandle::SharedPtr callback_handle_;
+};
+
+
+int main(int argc, char **argv)
+{
+    rclcpp::init(argc, argv);
+    rclcpp::spin(std::make_shared<ThrusterController>());
+    rclcpp::shutdown();
+    return 0;
+}

test/test_urdf_files.py

+#!/usr/bin/env python
+# Copyright (c) 2020 The Plankton Authors.
+# All rights reserved.
+#
+# This source code is derived from UUV Simulator
+# (https://github.com/uuvsimulator/uuv_simulator)
+# Copyright (c) 2016-2019 The UUV Simulator Authors
+# licensed under the Apache license, Version 2.0
+# cf. 3rd-party-licenses.txt file in the root directory of this source tree.
+#
+# 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 unittest
+import subprocess
+import os
+
+
+def call_xacro(xml_file):
+    assert os.path.isfile(xml_file), 'Invalid XML xacro file'
+    return subprocess.check_output(['xacro', '--inorder', xml_file])
+
+
+# =============================================================================
+class TestRexROVURDFFiles(unittest.TestCase):
+    def test_xacro(self):
+        # Retrieve the root folder for the tests
+        test_dir = os.path.abspath(os.path.dirname(__file__))
+        robots_dir = os.path.join(test_dir, '..', 'robots')
+
+        for item in os.listdir(robots_dir):
+            if 'oberon' in item:
+                continue
+            if not os.path.isfile(os.path.join(robots_dir, item)):
+                continue
+            output = call_xacro(os.path.join(robots_dir, item))
+
+            self.assertNotIn(
+                'XML parsing error',
+                output.decode('utf-8'), 
+                'Parsing error found for file {}'.format('hey'))
+            self.assertNotIn(
+                'No such file or directory', 
+                output.decode('utf-8'), 
+                'Some file not found in {}'.format('hey'))
+
+
+
+

urdf/sensor_snippets.xacro

+<?xml version="1.0"?>
+
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
+
+    <xacro:macro name="default_camera_lightweight" params="namespace parent_link suffix rate hfov width height *origin">
+        <xacro:regular_camera_plugin_macro
+            namespace="${namespace}"
+            suffix="${suffix}"
+            parent_link="${parent_link}"
+            topic="camera"
+            mass="0.001"
+            update_rate="${rate}"
+            hfov="${hfov}"
+            width="${width}"
+            height="${height}"
+            stddev="0.0"
+            scale="1">
+            <inertia ixx="0.00001" ixy="0.0" ixz="0.0" iyy="0.00001" iyz="0.0" izz="0.00001" />
+            <xacro:insert_block name="origin" />
+        </xacro:regular_camera_plugin_macro>
+    </xacro:macro>
+
+    <xacro:macro name="no_collision">
+        <collision>
+            <geometry>
+                <cylinder length="${0.000001}" radius="${0.000001}" />
+            </geometry>
+            <origin xyz="0 0 0" rpy="0 ${0.5*pi} 0"/>
+        </collision>
+    </xacro:macro>
+
+    <xacro:macro name="default_echosounder" params="namespace parent_link *origin suffix">
+        <xacro:echosounder_plugin_macro
+            namespace="${namespace}"
+            suffix="${suffix}"
+            parent_link="${parent_link}"
+            update_rate="7"
+            topic="echosounder"
+            scale="0.1">
+            <xacro:insert_block name="origin" />
+        </xacro:echosounder_plugin_macro>
+    </xacro:macro>
+
+    <xacro:macro name="echosounder_plugin_macro"
+                 params="namespace suffix parent_link topic scale *origin update_rate">
+
+        <link name="${namespace}/echosounder${suffix}_link">
+            <inertial>
+                <mass value="0.001" />  <!-- [kg] -->
+                <origin xyz="0 0 0" rpy="0 0 0" />
+                <inertia ixx="0.000000017" ixy="0" ixz="0" iyy="0.000000017" iyz="0" izz="0.000000017" />
+            </inertial>
+            <visual>
+                <geometry>
+                    <mesh filename="file://$(find uuv_sensor_ros_plugins)/meshes/dvl.dae" scale="${scale} ${scale} ${scale}"/>
+                </geometry>
+            </visual>
+            <xacro:no_collision/>
+        </link>
+
+        <joint name="${namespace}/echosounder${suffix}_joint" type="revolute">
+            <xacro:insert_block name="origin" />
+            <parent link="${parent_link}" />
+            <child link="${namespace}/echosounder${suffix}_link" />
+            <limit upper="0" lower="0" effort="0" velocity="0" />
+            <axis xyz="1 0 0"/>
+        </joint>
+
+        <gazebo reference="${namespace}/echosounder${suffix}_link">
+            <sensor type="ray" name="echosounder_{suffix}">
+                <pose>0 0 0 0 0 0</pose>
+                <visualize>false</visualize>
+                <update_rate>${update_rate}</update_rate>
+                <ray>
+                    <scan>
+                        <horizontal>
+                            <samples>1</samples>
+                            <resolution>1</resolution>
+                            <min_angle>-0.087</min_angle>
+                            <max_angle>0.087</max_angle>
+                        </horizontal>
+                    </scan>
+                    <range>
+                        <min>0.25</min>
+                        <max>150.0</max>
+                        <resolution>0.01</resolution>
+                    </range>
+                    <noise>
+                        <type>gaussian</type>
+                        <mean>0.0</mean>
+                        <stddev>0.01</stddev>
+                    </noise>
+                </ray>
+
+                <plugin name="gazebo_ros_echosounder${suffix}_plugin" filename="libgazebo_ros_ray_sensor.so">
+                    <ros>
+                        <namespace>${namespace}</namespace>
+                        <remapping>~/out:=${topic}</remapping>
+                    </ros>
+                    <output_type>sensor_msgs/Range</output_type>
+                    <radiation_type>ultrasound</radiation_type>
+                </plugin>
+            </sensor>
+        </gazebo>
+    </xacro:macro>
+</robot>

urdf/waterstrider.gazebo.xacro

+<?xml version="1.0"?>
+<!-- Copyright (c) 2020 The Plankton Authors.
+     All rights reserved.
+
+     This source code is derived from UUV Simulator
+     (https://github.com/uuvsimulator/uuv_simulator)
+     Copyright (c) 2016-2019 The UUV Simulator Authors
+     licensed under the Apache license, Version 2.0
+     cf. 3rd-party-licenses.txt file in the root directory of this source tree.
+
+     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.
+-->
+
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
+    <!--
+    Center of buoyancy according to eq. (3.1) p. 28 in Berg2012.
+    The original values, [0.0822, -0.00773, 0.3872] however, seem to
+    assume NWU (otherwise cob is below cog?).
+    -->
+    <xacro:property name="cob" value="0.0 0.0 0.01"/>
+
+    <!-- Volume, see p.31 in Berg2012 -->
+    <xacro:property name="volume" value="0.011054"/>
+
+    <xacro:macro name="waterstrider_hydro_model" params="namespace">
+        <link name="${namespace}/base_link">
+          <neutrally_buoyant>0</neutrally_buoyant>
+          <volume>${volume}</volume>
+          <box>
+              <length>0.726</length>
+              <width>0.448</width>
+              <height>0.256</height>
+          </box>
+          <center_of_buoyancy>${cob}</center_of_buoyancy>
+            <metacentric_width>0.224</metacentric_width>
+            <metacentric_length>50</metacentric_length>
+            <submerged_height>0.12</submerged_height>
+            <hydrodynamic_model>
+                <type>fossen</type>
+                <added_mass>
+                     5   0   0   0   0   0
+                     0   5   0   0   0   0
+                     0   0   0.1 0   0   0
+                     0   0   0   0.1 0   0
+                     0   0   0   0   0.1 0
+                     0   0   0   0   0   0.1
+                </added_mass>
+                <linear_damping>
+                    -0 -20 -20 -20 -20 -1.5
+                </linear_damping>
+                <quadratic_damping>
+                    -20.0 -10.0 -10.0 -50.0 -50.0 -7.0
+                </quadratic_damping>
+            </hydrodynamic_model>
+
+        </link>
+    </xacro:macro>
+</robot>

urdf/waterstrider_actuators.xacro

+<?xml version="1.0"?>
+<!-- Copyright (c) 2020 The Plankton Authors.
+     All rights reserved.
+
+     This source code is derived from UUV Simulator
+     (https://github.com/uuvsimulator/uuv_simulator)
+     Copyright (c) 2016-2019 The UUV Simulator Authors
+     licensed under the Apache license, Version 2.0
+     cf. 3rd-party-licenses.txt file in the root directory of this source tree.
+
+     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.
+-->
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
+
+  <xacro:property name="prop_mesh_file" value="file://$(find waterstridergazebo)/meshes/untitled.obj"/>
+
+  <!-- Thruster joint and link snippet -->
+  <xacro:macro name="thruster_macro"
+    params="namespace thruster_id *origin">
+    <xacro:thruster_module_first_order_basic_fcn_macro
+      namespace="${namespace}"
+      thruster_id="${thruster_id}"
+      mesh_filename="${prop_mesh_file}"
+      dyn_time_constant="0.1"
+      rotor_constant="0.00124">
+      <xacro:insert_block name="origin"/>
+    </xacro:thruster_module_first_order_basic_fcn_macro>
+  </xacro:macro>
+
+ <xacro:thruster_macro namespace="${namespace}" thruster_id="0">
+    <origin xyz="-0.22795 -0.150 -0.08897" rpy="0 0 ${180*d2r}"/>
+  </xacro:thruster_macro>
+  <xacro:thruster_macro namespace="${namespace}" thruster_id="1">
+    <origin xyz="-0.22795 0.150 -0.08897" rpy="0 0 ${180*d2r}"/>
+  </xacro:thruster_macro>
+</robot>

urdf/waterstrider_base.xacro

+<?xml version="1.0"?>
+<!-- Copyright (c) 2020 The Plankton Authors.
+     All rights reserved.
+
+     This source code is derived from UUV Simulator
+     (https://github.com/uuvsimulator/uuv_simulator)
+     Copyright (c) 2016-2019 The UUV Simulator Authors
+     licensed under the Apache license, Version 2.0
+     cf. 3rd-party-licenses.txt file in the root directory of this source tree.
+
+     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.
+-->
+
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
+  <!--
+      The RexROV meshes and hydrodynamic parameters are based on the parameters and dimensions
+      from the SF 30k.
+
+      References:
+        [1] Dukan, Fredrik. "ROV Motion Control Systems." PhD Thesis, NTNU, 2014.
+        [2] Berg, Viktor. Development and Commissioning of a DP system for ROV SF 30k. MS
+            thesis. Institutt for marin teknikk, 2012.
+  -->
+  <!-- includes -->
+  <xacro:include filename="$(find uuv_descriptions)/urdf/common.urdf.xacro"/>
+  <xacro:include filename="$(find uuv_sensor_ros_plugins)/urdf/sensor_snippets.xacro"/>
+  <xacro:include filename="$(find uuv_gazebo_ros_plugins)/urdf/snippets.xacro"/>
+  <xacro:include filename="$(find waterstridergazebo)/urdf/sensor_snippets.xacro" />
+
+  <!-- Parameters -->
+  <xacro:property name="namespace" value="waterstrider"/>
+  <xacro:property name="visual_mesh_file" value="file://$(find waterstridergazebo)/meshes/waterstrider.obj"/>
+  <xacro:property name="prop_mesh_file" value="file://$(find waterstridergazebo)/meshes/prop.dae"/>
+
+  <!-- Mass, see p.28 in Berg2012 -->
+  <xacro:property name="mass" value="10.0"/>
+
+  <xacro:macro name="waterstrider_base" params="namespace *gazebo inertial_reference_frame">
+    <!-- base_link according to ROS conventions: x forward, z up -->
+    <link name="${namespace}/base_link">
+      <origin xyz="0 0 0" rpy="0 0 ${90*d2r}"/>
+      <inertial>
+        <mass value="${mass}"/>
+        <origin xyz="0 0 0" rpy="0 0 0"/>
+        <inertia ixx="0.30923088" ixy="0.00325024" ixz="0.00234528" 
+                 iyy="0.57808352" iyz="0.00329168" izz="0.4852848"/>
+      </inertial>
+      <visual>
+        <origin xyz="0 0 0.03" rpy="${90*d2r} 0 ${90*d2r}"/>
+        <geometry>
+          <mesh filename="${visual_mesh_file}" scale="0.001 0.001 0.001"/>
+        </geometry>
+      </visual>
+      <collision>
+        <origin xyz="0.05 -0.215 0" rpy="0 0 0"/>
+        <geometry>
+          <box size="0.726 0.05 0.283"/>
+        </geometry>
+      </collision>
+
+      <collision>
+        <origin xyz="0.05 0.215 0" rpy="0 0 0"/>
+        <geometry>
+          <box size="0.726 0.05 0.283"/>
+        </geometry>
+      </collision>
+    </link>
+
+    <!-- Set up hydrodynamic plugin given as input parameter -->
+    <xacro:insert_block name="gazebo"/>
+    <!-- optional: plugin to test compare Gazebo returned accelerations
+    <gazebo>
+      <plugin name="${namespace}_test_plugin" filename="libuuv_accelerations_test_plugin.so">
+        <link_name>${namespace}/base_link</link_name>
+      </plugin>
+    </gazebo> -->
+
+    <xacro:include filename="$(find waterstridergazebo)/urdf/waterstrider_actuators.xacro"/>
+    <xacro:include filename="$(find waterstridergazebo)/urdf/waterstrider_sensors.xacro"/>
+
+  </xacro:macro>
+</robot>

urdf/waterstrider_sensors.xacro

+<?xml version="1.0"?>
+<!-- Copyright (c) 2020 The Plankton Authors.
+     All rights reserved.
+
+     This source code is derived from UUV Simulator
+     (https://github.com/uuvsimulator/uuv_simulator)
+     Copyright (c) 2016-2019 The UUV Simulator Authors
+     licensed under the Apache license, Version 2.0
+     cf. 3rd-party-licenses.txt file in the root directory of this source tree.
+
+     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.
+-->
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
+  <!-- Mount a camera -->
+  <!--fov 90 1.5707963267948966, 120 2.0943951023931953 (bad work), 170 2.9670597283903604 (bad work)-->
+  <xacro:default_camera_lightweight 
+    namespace="${namespace}" 
+    parent_link="${namespace}/base_link" 
+    suffix="_front"
+    rate="20"
+    hfov="2.0943951023931953" 
+    width="640"
+    height="480">
+    <origin xyz="0.10 0 0.25" rpy="0 0 0"/>
+  </xacro:default_camera_lightweight>
+
+  <xacro:default_camera_lightweight 
+    namespace="${namespace}"
+    parent_link="${namespace}/base_link"
+    suffix="_down"
+    rate="20"
+    hfov="2.0943951023931953"
+    width="640"
+    height="480">
+    <origin xyz="-0.2 0 -0.05" rpy="0 ${90 * d2r} 0"/>
+  </xacro:default_camera_lightweight>
+
+</robot>