diff --git a/uuv_assistants/templates/robot_model/urdf/snippets.xacro.template b/uuv_assistants/templates/robot_model/urdf/snippets.xacro.template
index ad83cb67..50b4dea8 100644
--- a/uuv_assistants/templates/robot_model/urdf/snippets.xacro.template
+++ b/uuv_assistants/templates/robot_model/urdf/snippets.xacro.template
@@ -201,15 +201,15 @@
-
+
-
+
-
+
@@ -279,8 +279,9 @@
/hydrodynamics/current_velocity
- ${namespace}/fin${fin_id}
- ${namespace}/fin${fin_id}_joint
+ ${namespace}/fin_${fin_id}
+ ${namespace}/fin_${fin_id}_joint
+ ${fin_id}
${namespace}/fins/${fin_id}/output
diff --git a/uuv_control/uuv_auv_control_allocator/config/actuator_manager.yaml b/uuv_control/uuv_auv_control_allocator/config/actuator_manager.yaml
new file mode 100644
index 00000000..101d2a31
--- /dev/null
+++ b/uuv_control/uuv_auv_control_allocator/config/actuator_manager.yaml
@@ -0,0 +1,64 @@
+# $ROBOT_NAME:
+ auv_control_allocator:
+ ros__parameters:
+ base_link: base_link
+ update_rate: 50
+ timeout: -1
+
+ thruster_config:
+ topic_prefix: thrusters/
+ topic_suffix: /input
+ frame_base: thruster_
+ max_thrust:
+
+ ##################################################
+ # 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:
+ #
+ # Basic
+ # 0.0
+ #
+ # You can set the conversion function to be:
+ conversion_fcn: proportional
+ conversion_fcn_params:
+ gain:
+
+ # 1.2) If the conversion function set for the thruster plugins is the following:
+ #
+ # LinearInterp
+ # 0 1 2 3
+ # 0 1 2 3
+ #
+ # 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]
+
+ fin_config:
+ # Fin Configuration
+ topic_prefix: fins/
+ topic_suffix: /input
+ frame_base: fin_
+ lower_limit: 0.0 # Radians
+ upper_limit: 0.0
+ fin_area: 0.0
+ fluid_density: 1028.0
+ lift_coefficient: 0.0
\ No newline at end of file
diff --git a/uuv_control/uuv_auv_control_allocator/launch/start_control_allocator.launch.py b/uuv_control/uuv_auv_control_allocator/launch/start_control_allocator.launch.py
new file mode 100644
index 00000000..522be295
--- /dev/null
+++ b/uuv_control/uuv_auv_control_allocator/launch/start_control_allocator.launch.py
@@ -0,0 +1,50 @@
+import os
+import sys
+
+from launch import LaunchDescription
+from launch.actions.declare_launch_argument import DeclareLaunchArgument
+from launch_ros.actions import Node, PushRosNamespace
+from launch.actions.opaque_function import OpaqueFunction
+from launch.substitutions import LaunchConfiguration as LC
+from launch.actions import GroupAction
+
+import launch
+import launch.actions
+
+from ament_index_python.packages import get_package_share_directory
+
+bool_aliases = [1, '1', "True", "true"]
+
+def launch_setup(context, *args, **kwargs):
+ tam_reset_arg = {"output_dir": LC("output_dir").perform(context)} if (LC("reset_tam").perform(context) in bool_aliases) else LC("tam_config").perform(context)
+ fin_manager_node = Node(
+ name="auv_control_allocator",
+ package="uuv_auv_control_allocator",
+ executable="control_allocator",
+ output="screen",
+ parameters=[LC("config_file").perform(context)],
+ remappings=[("tf", "/tf")])
+
+ group = GroupAction([
+ PushRosNamespace(LC("namespace").perform(context)), # replace with vehicle namespace
+ fin_manager_node
+ ])
+
+ return [group]
+
+def generate_launch_description():
+
+ ld = launch.LaunchDescription([
+ DeclareLaunchArgument("namespace", default_value="ROBOT_NAME"), # Add your robot name here
+ DeclareLaunchArgument("reset_tam", default_value="True"), # TODO: Fix TAM Matrix Storage and Loading, need TAM?
+ DeclareLaunchArgument("config_file", default_value=os.path.join(get_package_share_directory("uuv_auv_control_allocator"), "config", "actuator_manager.yaml")),
+ #DeclareLaunchArgument("tam_config", default_value=os.path.join(get_package_share_directory("uuv_auv_control_allocator"), "config", "TAM.yaml")),
+ DeclareLaunchArgument("output_dir", default_value=os.path.join(get_package_share_directory("uuv_auv_control_allocator"), "config")),
+ OpaqueFunction(function=launch_setup)
+ ])
+
+ return ld
+
+
+if __name__ == '__main__':
+ generate_launch_description()
diff --git a/uuv_control/uuv_auv_control_allocator/scripts/control_allocator b/uuv_control/uuv_auv_control_allocator/scripts/control_allocator
index 5a117f81..157a92a5 100755
--- a/uuv_control/uuv_auv_control_allocator/scripts/control_allocator
+++ b/uuv_control/uuv_auv_control_allocator/scripts/control_allocator
@@ -33,16 +33,9 @@ from uuv_auv_actuator_interface import ActuatorManager
from plankton_utils.time import time_in_float_sec
from plankton_utils.time import is_sim_time
-
-# TODO Probably refactor
class AUVControlAllocator(ActuatorManager):
- def __init__(self, node_name, **kwargs):
- #super(AUVControlAllocator, self).__init__()
- ActuatorManager.__init__(
- node_name,
- **kwargs
- )
-
+ def __init__(self, name, **kwargs):
+ ActuatorManager.__init__(self, name, **kwargs)
# Retrieve the output file path to store the CAM
# matrix for future use
self.output_dir = None
@@ -53,11 +46,12 @@ class AUVControlAllocator(ActuatorManager):
'Invalid output directory, output_dir=' + self.output_dir)
self.get_logger().info('output_dir=' + str(self.output_dir))
- self.update_rate = self.get_parameter('update_rate', 10).value
+ self.update_rate = self.get_parameter('update_rate').value
+
if self.update_rate <= 0:
raise RuntimeError('Update rate must be a positive number')
- self.timeout = self.get_parameter('timeout', -1).value
+ self.timeout = self.get_parameter('timeout').value
# Control allocation matrix for thruster and fin lift forces
self.u = casadi.SX.sym('u')
@@ -69,29 +63,40 @@ class AUVControlAllocator(ActuatorManager):
self.input_surge_speed = 0.0
self.input_tau = np.zeros(6)
- if not self.init():
+ self.init_future.add_done_callback(self._init_class)
+ # =========================================================================
+ def _init_class(self, future):
+ """
+ Initializes the class. Called as a callback when the parent class
+ is initialized
+ """
+ try:
+ self._init_class_impl()
+ except Exception as e:
+ self.get_logger().info('Caught exception: ' + repr(e))
+ # =========================================================================
+ def _init_class_impl(self):
+ if not self.update_cam():
raise RuntimeError('No thruster and/or fins found')
- self.sub = self.create_subscription(AUVCommand, 'control_input', self.control_callback, 10)
+ self.sub = self.create_subscription(AUVCommand, "/" + self.namespace + '/control_input', self.control_callback, 10)
# NB : no spin before, the sim time clock() is automatically 0
self.last_update = time_in_float_sec(self.get_clock().now())
- rate = self.create_rate(self.update_rate)
- thread = threading.Thread(target=rclpy.spin, args=(self,), daemon=True)
- thread.start()
- while rclpy.ok():
- #rclpy.spin_once(self)
- if self.timeout > -1:
- if time_in_float_sec(self.get_clock().now()) - self.last_update > self.timeout:
- self.input_surge_speed = 0.0
- self.input_tau = np.zeros(6)
- output = self.allocate(self.input_tau, self.input_surge_speed)
- self.publish_commands(output)
- rate.sleep()
+ self.timer_function = self.create_timer(1.0 / self.update_rate, self.spin_function)
+ self.get_logger().info('ControlAllocator: ready')
# =========================================================================
- def init(self):
+ def spin_function(self):
+ if self.timeout > -1:
+ if time_in_float_sec(self.get_clock().now()) - self.last_update > self.timeout:
+ self.input_surge_speed = 0.0
+ self.input_tau = np.zeros(6)
+ output = self.allocate(self.input_tau, self.input_surge_speed)
+ self.publish_commands(output)
+ # =========================================================================
+ def update_cam(self):
"""Calculate the control allocation matrix, if one is not given."""
# Build the casadi model
@@ -155,7 +160,7 @@ class AUVControlAllocator(ActuatorManager):
self.input_tau[0:3] = np.dot(self.base_link_ned_to_enu, self.input_tau[0:3])
self.input_tau[4::] = np.dot(self.base_link_ned_to_enu, self.input_tau[4::])
- self.last_update = self.get_clock().now()
+ self.last_update = time_in_float_sec(self.get_clock().now())
# =============================================================================
@@ -166,15 +171,14 @@ def main():
sim_time_param = is_sim_time()
node = AUVControlAllocator('auv_control_allocator', parameter_overrides=[sim_time_param])
-
- except rclpy.ROSInterruptException:
- print('AUVControlAllocator::ROSInterruptException Exception')
+ rclpy.spin(node)
+
except Exception as e:
- print('Caught exception: ' + str(e))
+ print('AUVControlAllocatoreNode::Exception: ' + str(e))
finally:
if rclpy.ok():
rclpy.shutdown()
- print('Leaving AUVControlAllocator')
+ print('Leaving AUVControlAllocatorNode')
# =============================================================================
diff --git a/uuv_control/uuv_auv_control_allocator/src/uuv_auv_actuator_interface/actuator_manager.py b/uuv_control/uuv_auv_control_allocator/src/uuv_auv_actuator_interface/actuator_manager.py
index 2ffabea1..fed3b4c7 100644
--- a/uuv_control/uuv_auv_control_allocator/src/uuv_auv_actuator_interface/actuator_manager.py
+++ b/uuv_control/uuv_auv_control_allocator/src/uuv_auv_actuator_interface/actuator_manager.py
@@ -21,6 +21,7 @@
import numpy as np
import os
import yaml
+import time
from geometry_msgs.msg import Wrench, WrenchStamped
import rclpy
@@ -34,9 +35,9 @@
from uuv_auv_control_allocator.msg import AUVCommand
from uuv_gazebo_ros_plugins_msgs.msg import FloatStamped
from .fin_model import FinModel
-from plankton_utils.params_helper import parse_nested_params_to_dict
+from plankton_utils.param_helper import parse_nested_params_to_dict
-#TODO Refactor
+import threading
class ActuatorManager(Node):
MAX_FINS = 4
@@ -52,40 +53,13 @@ def __init__(self, name, **kwargs):
self.tf_buffer = tf2_ros.Buffer()
self.listener = tf2_ros.TransformListener(self.tf_buffer, self)
- tf_trans_ned_to_enu = None
-
- try:
- if self.namespace != '':
- target = '{}/base_link'.format(self.namespace)
- source = '{}/base_link_ned'.format(self.namespace)
- else:
- target = 'base_link'
- source = 'base_link_ned'
- self.get_logger().info('Lookup transfrom from %s to %s' % (source, target))
- tf_trans_ned_to_enu = self.tf_buffer.lookup_transform().lookup_transform(
- target, source, rclpy.time.Time(), rclpy.time.Duration(seconds=1))
- except Exception as e:
- self.get_logger().warning('No transform found between base_link and base_link_ned'
- ' for vehicle {}, message={}'.format(self.namespace, e))
- self.base_link_ned_to_enu = None
-
- if tf_trans_ned_to_enu is not None:
- self.base_link_ned_to_enu = quaternion_matrix(
- (tf_trans_ned_to_enu.transform.rotation.x,
- tf_trans_ned_to_enu.transform.rotation.y,
- tf_trans_ned_to_enu.transform.rotation.z,
- tf_trans_ned_to_enu.transform.rotation.w))[0:3, 0:3]
-
- self.get_logger().warning('base_link transform NED to ENU=\n{}'.format(
- self.base_link_ned_to_enu))
-
- self.base_link = self.get_parameter('base_link', 'base_link').get_parameter_value().string_value
+ self.base_link = self.get_parameter('base_link').get_parameter_value().string_value
# Retrieve the thruster configuration parameters if available
thruster_config = self.get_parameters_by_prefix('thruster_config')
if len(thruster_config) == 0:
raise RuntimeError('Thruster configuration not available')
- self.thruster_config = parse_nested_params_to_dict(self.thruster_config, '.', True)
+ self.thruster_config = parse_nested_params_to_dict(thruster_config, '.', True)
# Check if all necessary thruster model parameter are available
@@ -97,9 +71,9 @@ def __init__(self, name, **kwargs):
'Parameter <%s> for thruster conversion function is missing' % p)
# Setting up the thruster topic name
- self.thruster_topic = build_topic_name(self.namespace,
- self.thruster_config['topic_prefix'], 0,
- self.thruster_config['topic_suffix'])
+ self.thruster_topic = self.build_topic_name(self.thruster_config['topic_prefix'],
+ 0,
+ self.thruster_config['topic_suffix'])
self.thruster = None
@@ -109,7 +83,7 @@ def __init__(self, name, **kwargs):
raise RuntimeError('Fin configuration is not available')
- self.fin_config = parse_nested_params_to_dict(self.fin_config, '.', True)
+ self.fin_config = parse_nested_params_to_dict(fin_config, '.', True)
# Check if all necessary fin parameters are available
fin_params = ['fluid_density', 'lift_coefficient', 'fin_area',
@@ -134,23 +108,64 @@ def __init__(self, name, **kwargs):
self.fins = dict()
self.n_fins = 0
+
+ self.init_future = rclpy.Future()
+ self.init_thread = threading.Thread(target=self._init_async, daemon=True)
+ self.init_thread.start()
+ # =========================================================================
+ def _init_async(self):
+ try:
+ self._init_async_impl()
+ except Exception as e:
+ self.get_logger().warn('Caught exception: ' + repr(e))
+ # =========================================================================
+ def _init_async_impl(self):
+ tf_trans_ned_to_enu = None
+
+ try:
+ if self.namespace != '':
+ target = '{}/base_link'.format(self.namespace)
+ source = '{}/base_link_ned'.format(self.namespace)
+ else:
+ target = 'base_link'
+ source = 'base_link_ned'
+
+ self.get_logger().info('Lookup transform from %s to %s' % (source, target))
+ tf_trans_ned_to_enu = self.tf_buffer.lookup_transform(
+ target, source, rclpy.time.Time(), rclpy.time.Duration(seconds=10))
+ except Exception as e:
+ self.get_logger().warning('No transform found between base_link and base_link_ned'
+ ' for vehicle {}, message={}'.format(self.namespace, e))
+ self.base_link_ned_to_enu = None
+ if tf_trans_ned_to_enu is not None:
+ self.base_link_ned_to_enu = quaternion_matrix(
+ (tf_trans_ned_to_enu.transform.rotation.x,
+ tf_trans_ned_to_enu.transform.rotation.y,
+ tf_trans_ned_to_enu.transform.rotation.z,
+ tf_trans_ned_to_enu.transform.rotation.w))[0:3, 0:3]
+
+ self.get_logger().warning('base_link transform NED to ENU=\n{}'.format(
+ self.base_link_ned_to_enu))
if not self.find_actuators():
raise RuntimeError('No thruster and/or fins found')
+ self._ready = True
+ self.init_future.set_result(True)
+ self.get_logger().info('ActuatorManager: ready')
# =========================================================================
def find_actuators(self):
"""Calculate the control allocation matrix, if one is not given."""
self.ready = False
- self.get_logger().infos('ControlAllocator: updating thruster poses')
+ self.get_logger().info('ControlAllocator: updating thruster poses')
base = '%s/%s' % (self.namespace, self.base_link)
frame = '%s/%s%d' % (self.namespace, self.thruster_config['frame_base'], 0)
self.get_logger().info('Lookup: Thruster transform found %s -> %s' % (base, frame))
- trans = self.tf_buffer.lookup_transform(base, frame, rclpy.time.Time(), rclpy.time.Duration(seconds=1))
+ trans = self.tf_buffer.lookup_transform(base, frame, rclpy.time.Time(), rclpy.time.Duration(seconds=10))
pos = np.array([trans.transform.translation.x,
trans.transform.translation.y,
trans.transform.translation.z])
@@ -165,6 +180,7 @@ def find_actuators(self):
# Read transformation from thruster
#params = {key: val.value for key, val in params.items()}
self.thruster = Thruster.create_thruster(
+ self,
self.thruster_config['conversion_fcn'], 0,
self.thruster_topic, pos, quat,
**self.thruster_config['conversion_fcn_params'])
@@ -174,7 +190,7 @@ def find_actuators(self):
frame = '%s/%s%d' % (self.namespace, self.fin_config['frame_base'], i)
self.get_logger().info('Lookup: Fin transform found %s -> %s' % (base, frame))
- trans = self.tf_buffer.lookup_transform(base, frame, rclpy.time.Time(), rclpy.time.Duration(seconds=1))
+ trans = self.tf_buffer.lookup_transform(base, frame, rclpy.time.Time(), rclpy.time.Duration(seconds=10))
pos = np.array([trans.transform.translation.x,
trans.transform.translation.y,
trans.transform.translation.z])
@@ -186,8 +202,9 @@ def find_actuators(self):
self.get_logger().info('pos=' + str(pos))
self.get_logger().info('quat=' + str(quat))
- fin_topic = build_topic_name(self.namespace,
- self.fin_config['topic_prefix'], i, self.fin_config['topic_suffix'])
+ fin_topic = self.build_topic_name(self.fin_config['topic_prefix'],
+ i,
+ self.fin_config['topic_suffix'])
self.fins[i] = FinModel(
i,
@@ -204,9 +221,9 @@ def find_actuators(self):
self.get_logger().info('# fins found: %d' % len(self.fins.keys()))
for i in range(self.n_fins):
- self.get_logger().info(i)
- self.get_logger().info(self.fins[i].pos)
- self.get_logger().info(self.fins[i].rot)
+ self.get_logger().info(str(i))
+ self.get_logger().info('pos=' + str(self.fins[i].pos))
+ self.get_logger().info('quat=' + str(self.fins[i].rot))
self.ready = True
return True
@@ -234,5 +251,5 @@ def publish_commands(self, command):
self.fins[i].publish_command(command[i + 1])
# =========================================================================
- def build_topic_name(self, namespace, topic_prefix, id, topic_prefix):
- return '/%s/%s/id_%d/%s' % (namespace, topic_prefix, 0, topic_suffix)
+ def build_topic_name(self, prefix, id, suffix):
+ return prefix + 'id_' + str(id) + suffix
\ No newline at end of file
diff --git a/uuv_control/uuv_auv_control_allocator/src/uuv_auv_actuator_interface/fin_model.py b/uuv_control/uuv_auv_control_allocator/src/uuv_auv_actuator_interface/fin_model.py
index 6f71bddb..57603bf9 100644
--- a/uuv_control/uuv_auv_control_allocator/src/uuv_auv_actuator_interface/fin_model.py
+++ b/uuv_control/uuv_auv_control_allocator/src/uuv_auv_actuator_interface/fin_model.py
@@ -46,6 +46,6 @@ def __init__(self, index, pos, quat, topic, node:rclpy.node.Node):
# =========================================================================
def publish_command(self, delta):
msg = FloatStamped()
- msg.header.stamp = self.node.get_clock().now()
+ msg.header.stamp = self.node.get_clock().now().to_msg()
msg.data = delta
self.pub.publish(msg)
diff --git a/uuv_gazebo_plugins/uuv_gazebo_plugins/include/uuv_gazebo_plugins/FinPlugin.h b/uuv_gazebo_plugins/uuv_gazebo_plugins/include/uuv_gazebo_plugins/FinPlugin.h
index 6616af2e..7766a31b 100644
--- a/uuv_gazebo_plugins/uuv_gazebo_plugins/include/uuv_gazebo_plugins/FinPlugin.h
+++ b/uuv_gazebo_plugins/uuv_gazebo_plugins/include/uuv_gazebo_plugins/FinPlugin.h
@@ -90,9 +90,12 @@ class FinPlugin : public ModelPlugin
/// \brief Subscriber to the reference signal topic.
protected: transport::SubscriberPtr commandSubscriber;
- /// \brief Publisher to the output thrust topic
+ /// \brief Publisher to the output angle topic
protected: transport::PublisherPtr anglePublisher;
+ /// \brief Publisher to the output wrench topic
+ protected: transport::PublisherPtr wrenchPublisher;
+
/// \brief Force component calculated from the lift and drag module
protected: ignition::math::Vector3d finForce;
diff --git a/uuv_gazebo_plugins/uuv_gazebo_plugins/src/FinPlugin.cpp b/uuv_gazebo_plugins/uuv_gazebo_plugins/src/FinPlugin.cpp
index 22aab175..95cd6b6b 100644
--- a/uuv_gazebo_plugins/uuv_gazebo_plugins/src/FinPlugin.cpp
+++ b/uuv_gazebo_plugins/uuv_gazebo_plugins/src/FinPlugin.cpp
@@ -74,17 +74,12 @@ void FinPlugin::Load(physics::ModelPtr _model, sdf::ElementPtr _sdf)
myTopicPrefix = BuildTopicPrefix(_model->GetName(), this->finID);
// Input/output topics
- std::string inputTopic, outputTopic;
- if (_sdf->HasElement("input_topic"))
- std::string inputTopic = _sdf->Get("input_topic");
- else
- inputTopic = myTopicPrefix + "/input";
-
- if (_sdf->HasElement("output_topic"))
- outputTopic = _sdf->Get("output_topic");
- else
- outputTopic = myTopicPrefix + "/output";
-
+ std::string inputTopic, outputTopic, wrenchTopic, currentVelocityTopic;
+ inputTopic = _sdf->Get("input_topic");
+ outputTopic = _sdf->Get("output_topic");
+ wrenchTopic = _sdf->Get("wrench_topic");
+ currentVelocityTopic = _sdf->Get("current_velocity_topic");
+
GZ_ASSERT(_sdf->HasElement("link_name"), "Could not find link_name.");
std::string link_name = _sdf->Get("link_name");
this->link = _model->GetLink(link_name);
@@ -109,8 +104,6 @@ void FinPlugin::Load(physics::ModelPtr _model, sdf::ElementPtr _sdf)
// Subscribe to current velocity topic
GZ_ASSERT(_sdf->HasElement("current_velocity_topic"),
"Could not find current_velocity_topic.");
- std::string currentVelocityTopic =
- _sdf->Get("current_velocity_topic");
GZ_ASSERT(!currentVelocityTopic.empty(),
"Fluid velocity topic tag cannot be empty");
@@ -124,6 +117,9 @@ void FinPlugin::Load(physics::ModelPtr _model, sdf::ElementPtr _sdf)
this->anglePublisher = this->node->Advertise<
uuv_gazebo_plugins_msgs::msgs::Double>(outputTopic);
+ // Advertise the output topic
+ this->wrenchPublisher = this->node->Advertise<
+ uuv_gazebo_plugins_msgs::msgs::Double>(wrenchTopic);
// Subscribe to the input signal topic
this->commandSubscriber = this->node->Subscribe(inputTopic,
diff --git a/uuv_gazebo_plugins/uuv_gazebo_ros_plugins/src/FinROSPlugin.cpp b/uuv_gazebo_plugins/uuv_gazebo_ros_plugins/src/FinROSPlugin.cpp
index a63dd3e9..cc582321 100644
--- a/uuv_gazebo_plugins/uuv_gazebo_ros_plugins/src/FinROSPlugin.cpp
+++ b/uuv_gazebo_plugins/uuv_gazebo_ros_plugins/src/FinROSPlugin.cpp
@@ -116,7 +116,8 @@ void FinROSPlugin::Load(gazebo::physics::ModelPtr _parent, sdf::ElementPtr _sdf)
<< " gazebo -s libgazebo_ros_api_plugin.so\n";
return;
}
-
+
+ try {
myRosNode = gazebo_ros::Node::CreateWithArgs(_sdf->Get("name"));// gazebo_ros::Node::Get(_sdf);
//Change the buildtopicprefix function ("/") if creating a namespaced node here
RCLCPP_INFO(myRosNode->get_logger(), "[FinROSPlugin] Namespace: " + std::string(myRosNode->get_namespace()));
@@ -130,15 +131,11 @@ void FinROSPlugin::Load(gazebo::physics::ModelPtr _parent, sdf::ElementPtr _sdf)
myPubState = myRosNode->create_publisher<
uuv_gazebo_ros_plugins_msgs::msg::FloatStamped
>(this->anglePublisher->GetTopic(), 10);
-
- std::string wrenchTopic;
- if (_sdf->HasElement("wrench_topic"))
- wrenchTopic = _sdf->Get("wrench_topic");
- else
- wrenchTopic = myTopicPrefix + "/wrench_topic";
-
+
myPubFinForce =
- myRosNode->create_publisher(wrenchTopic, 10);
+ myRosNode->create_publisher<
+ geometry_msgs::msg::WrenchStamped
+ >(this->wrenchPublisher->GetTopic(), 10);
// std::stringstream stream;
// stream << _parent->GetName() << "/fins/" << this->finID <<
@@ -157,6 +154,12 @@ void FinROSPlugin::Load(gazebo::physics::ModelPtr _parent, sdf::ElementPtr _sdf)
this->rosPublishConnection = gazebo::event::Events::ConnectWorldUpdateBegin(
std::bind(&FinROSPlugin::RosPublishStates, this));
+
+ }
+ catch(std::exception& e)
+ {
+ gzerr << "Exception when loading plugin: " << e.what() << "\n";
+ }
}
/////////////////////////////////////////////////