New UUV Addition

examples/dave_nodes/CMakeLists.txt

@@ -46,6 +46,7 @@ install(DIRECTORY config
 catkin_install_python(PROGRAMS
   src/simple_box_motion.py
   src/joy_thrusterop.py
+  src/virgil_thrusterop.py
   src/dvl_gradient_plot.py
   src/dvl_state_and_gradient_dsl.py
   src/dvl_state_and_gradient_uuvsim.py

examples/dave_nodes/launch/virgil_thrusterop.launch

@@ -0,0 +1,15 @@
+<?xml version="1.0"?>
+<launch>
+  <arg name="joy_id" default="0"/>
+  <arg name="namespace" default="virgil"/>
+
+  <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>
+
+  <node pkg="dave_nodes" type="virgil_thrusterop.py" name="virgil_thrusterop"
+  output="screen">
+    <param name="namespace" value="$(arg namespace)"/>
+  </node>
+</launch>

examples/dave_nodes/src/virgil_thrusterop.py

@@ -0,0 +1,102 @@
+#!/usr/bin/env python3
+'''
+Node to directly control the thrusters
+'''
+import os
+
+import rospy
+from sensor_msgs.msg import Joy
+
+from uuv_gazebo_ros_plugins_msgs.msg import FloatStamped
+
+class ThrusterOp:
+    def __init__(self, namespace='virgil'):        
+
+        # max thrusters used for movement
+        self.num_horiz_thrusters = 4
+        self.num_vert_thrusters = 3
+
+        # Put thruster gains into dict
+        self.gain_dict = dict()
+        self.gain_dict[1] = [1000.0, 1000.0, 1000.0]
+        self.gain_dict[0] = [-250.0, 250.0, -250.0, -250.0]
+        self.gain_dict[4] = [500.0, 500.0, 500.0, -500.0]
+        self.gain_dict[3] = [700.0, -700.0, -750.0, -750.0]
+
+        # Joystick to thruster i.d. mapping
+        # Keys are the joystick axes, publishers
+        self.joy2thrust = dict()
+
+        # up down
+        self.joy2thrust[1] = [rospy.Publisher('/%s/thrusters/%d/input'%(namespace,4), FloatStamped, queue_size=1),
+                              rospy.Publisher('/%s/thrusters/%d/input'%(namespace,5), FloatStamped, queue_size=1),
+                              rospy.Publisher('/%s/thrusters/%d/input'%(namespace,6), FloatStamped, queue_size=1)]
+        # rotate
+        self.joy2thrust[0] = [rospy.Publisher('/%s/thrusters/%d/input'%(namespace,0), FloatStamped, queue_size=1),
+                              rospy.Publisher('/%s/thrusters/%d/input'%(namespace,1), FloatStamped, queue_size=1),
+                              rospy.Publisher('/%s/thrusters/%d/input'%(namespace,2), FloatStamped, queue_size=1),
+                              rospy.Publisher('/%s/thrusters/%d/input'%(namespace,3), FloatStamped, queue_size=1)]
+
+        # c
+        self.joy2thrust[4] = [rospy.Publisher('/%s/thrusters/%d/input'%(namespace,0), FloatStamped, queue_size=1),
+                              rospy.Publisher('/%s/thrusters/%d/input'%(namespace,1), FloatStamped, queue_size=1),
+                              rospy.Publisher('/%s/thrusters/%d/input'%(namespace,2), FloatStamped, queue_size=1),
+                              rospy.Publisher('/%s/thrusters/%d/input'%(namespace,3), FloatStamped, queue_size=1)]
+
+        # d
+        self.joy2thrust[3] = [rospy.Publisher('/%s/thrusters/%d/input'%(namespace,0), FloatStamped, queue_size=1),
+                              rospy.Publisher('/%s/thrusters/%d/input'%(namespace,1), FloatStamped, queue_size=1),
+                              rospy.Publisher('/%s/thrusters/%d/input'%(namespace,2), FloatStamped, queue_size=1),
+                              rospy.Publisher('/%s/thrusters/%d/input'%(namespace,3), FloatStamped, queue_size=1)]
+
+
+
+        self.joy_sub = rospy.Subscriber('joy', Joy, self.joy_callback)
+
+        rate = rospy.Rate(10)
+        while not rospy.is_shutdown():
+            rate.sleep()
+
+
+    def joy_callback(self, joy):
+        # Need to combine horizontal thrusters before sending out the message
+        msg = FloatStamped()
+        aa_dict = dict()
+
+        for aa, ii in zip(joy.axes, range(len(joy.axes))):
+            aa_dict[ii] = aa
+
+        key_list = list(self.joy2thrust.keys())
+        msg_values = [0] * self.num_horiz_thrusters
+        vert_values = [0] * self.num_vert_thrusters
+
+        for key_val in key_list:
+            # Keep vertical thrusters separate
+            if key_val == 1:
+                for ii in range(len(self.gain_dict[key_val])):
+                    msg.data = aa_dict[key_val] * self.gain_dict[key_val][ii]
+                    self.joy2thrust[key_val][ii].publish(msg)
+            # Mix 4 corner thrusters
+            else:
+                for ii in range(len(self.gain_dict[key_val])):
+                    msg_values[ii] += aa_dict[key_val] * self.gain_dict[key_val][ii]
+
+        for ii in range(len(self.joy2thrust[key_list[-1]])):
+            msg.data = msg_values[ii]
+            self.joy2thrust[key_list[-1]][ii].publish(msg)
+
+
+if __name__ == '__main__':
+    # Start the node
+    node_name = os.path.splitext(os.path.basename(__file__))[0]
+    rospy.init_node(node_name)
+    rospy.loginfo('Starting [%s] node' % node_name)
+
+    # Get params
+    ns = 'virgil'
+    if rospy.has_param('~namespace'):
+        ns = rospy.get_param('~namespace')
+
+    teleop = ThrusterOp(namespace = ns)
+    rospy.spin()
+    rospy.loginfo('Shutting down [%s] node' % node_name)

examples/dave_robot_launch/launch/virgil.launch

@@ -0,0 +1,79 @@
+<?xml version="1.0"?>
+<launch>
+	<arg name="gui" default="true"/>
+	<arg name="paused" default="false"/>
+	<arg name="world_name" default="$(find dave_worlds)/worlds/dave_ocean_waves.world"/>
+	<arg name="namespace" default="virgil"/>
+	<arg name="velocity_control" default="false"/>
+	<arg name="joy_id" default="0"/>
+	<arg name="debug" default="false"/>
+	<arg name="verbose" default="false"/>
+	<arg name="x" default="6"/>
+	<arg name="y" default="4"/>
+	<arg name="z" default="-93"/>
+	<arg name="roll" default="0"/>
+	<arg name="pitch" default="0"/>
+	<arg name="yaw" default="0"/>
+
+	<!-- Use Gazebo's empty_world.launch with dave_ocean_waves.world -->
+	<include file="$(find gazebo_ros)/launch/empty_world.launch">
+		<arg name="world_name" value="$(arg world_name)"/>
+		<arg name="paused" value="$(arg paused)"/>
+		<arg name="use_sim_time" value="true"/>
+		<arg name="gui" value="$(arg gui)"/>
+		<arg name="headless" value="false"/>
+		<arg name="debug" value="$(arg debug)"/>
+		<arg name="verbose" value="$(arg verbose)"/>
+	</include>
+
+	<!-- Use NED frame north east down -->
+	<include file="$(find uuv_assistants)/launch/publish_world_ned_frame.launch"/>
+
+	<!-- World models -->
+	<!-- <node name="publish_world_models"
+        pkg="uuv_assistants"
+        type="publish_world_models.py"
+        output="screen">
+	    <rosparam subst_value="true">
+            meshes:
+                heightmap:
+                    mesh: package://uuv_gazebo_worlds/models/sand_heightmap/meshes/heightmap.dae
+                    model: sand_heightmap
+                seafloor:
+                    plane: [2000, 2000, 0.1]
+                    pose:
+                        position: [0, 0, -100]
+                north:
+                    plane: [0.1, 2000, 100]
+                    pose:
+                        position: [1000, 0, -50]
+                south:
+                    plane: [0.1, 2000, 100]
+                    pose:
+                        position: [-1000, 0, -50]
+                west:
+                    plane: [2000, 0.1, 100]
+                    pose:
+                        position: [0, -1000, -50]
+                east:
+                    plane: [2000, 0.1, 100]
+                    pose:
+                        position: [0, 1000, -50]
+	    </rosparam>
+  	</node> -->
+
+  	<include file="$(find virgil_description)/launch/upload_virgil.launch"/>
+
+  	<include file="$(find dave_nodes)/launch/virgil_thrusterop.launch">
+    	<arg name="joy_id" value="$(arg joy_id)"/>
+    	<arg name="namespace" value="$(arg namespace)"/>
+  	</include>
+
+  	<node name="spawn_virgil" pkg="gazebo_ros" type="spawn_model"
+  		respawn="false" output="screen"
+  		args="-urdf -x $(arg x) -y $(arg y) -z $(arg z) -R $(arg roll) -P $(arg pitch) -Y $(arg yaw) -model $(arg namespace) -param /$(arg namespace)/virgil"/>
+
+  	<node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" respawn="true" output="screen">
+  		<remap from="robot_description" to="/$(arg namespace)/virgil"/>
+  	</node>
+</launch>

urdf/robots/virgil_description/CMakeLists.txt

@@ -0,0 +1,11 @@
+cmake_minimum_required(VERSION 3.1.1)
+project(virgil_description)
+
+find_package(catkin REQUIRED)
+
+include_directories(
+# include
+# ${catkin_INCLUDE_DIRS}
+)
+
+

urdf/robots/virgil_description/launch/upload_virgil.launch

@@ -0,0 +1,6 @@
+<?xml version="1.0"?>
+<launch>
+	<arg name="namespace" default="virgil"/>
+	<arg name="debug" default="false"/>
+	<param name="/$(arg namespace)/virgil" command="$(find xacro)/xacro '$(find virgil_description)/urdf/virgil.xacro' debug:=$(arg debug) namespace:=$(arg namespace)"/>
+</launch>