Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Fix scripts and templates within UUV Assistants #40

Open
wants to merge 10 commits into
base: master
Choose a base branch
from
15 changes: 14 additions & 1 deletion uuv_assistants/scripts/create_new_robot_model
Original file line number Diff line number Diff line change
Expand Up @@ -20,7 +20,6 @@
# See the License for the specific language governing permissions and
# limitations under the License.

import roslib
import os
import rclpy
import argparse
Expand Down Expand Up @@ -58,6 +57,20 @@ if __name__ == '__main__':
sys.exit(-1)
print('Done!')

# Add installation directory for robot model into CMakeLists.txt of package
pkg_cmake_path = colcon_pkg + "/CMakeLists.txt"
with open(pkg_cmake_path, 'r') as f:
contents = f.readlines()

contents.insert(22, '\n')
contents.insert(23, "install(DIRECTORY launch meshes robots urdf\nDESTINATION share/${PROJECT_NAME}\n PATTERN \"*~\"\n EXCLUDE)")
contents.insert(24, '\n')

with open(pkg_cmake_path, "w") as f:
contents = "".join(contents)
f.write(contents)

# Copy templates for robot model
template_path = os.path.join(get_package_share_directory('uuv_assistants'), 'templates', 'robot_model')

for d in os.listdir(template_path):
Expand Down
39 changes: 25 additions & 14 deletions uuv_assistants/scripts/create_thruster_manager_configuration
Original file line number Diff line number Diff line change
Expand Up @@ -20,20 +20,17 @@
# See the License for the specific language governing permissions and
# limitations under the License.

import roslib
import os
import rclpy
import argparse
import sys
import shutil
from rospkg import RosPack

from ament_index_python.packages import get_package_share_directory

ROSPACK_INST = RosPack()

if __name__ == '__main__':
parser = argparse.ArgumentParser(description="Create new colcon package for a UUV robot description")
parser = argparse.ArgumentParser(description="Create new colcon package for a UUV control configuration")
parser.add_argument('--robot_name', type=str)
parser.add_argument('--output_dir', type=str, default=None)

Expand All @@ -46,23 +43,37 @@ if __name__ == '__main__':

if args.output_dir is None:
output_dir = args.robot_name + '_control'
print('Creating colcon package %s for the thruster manager configuration' % output_dir)
if os.path.exists(output_dir):
print('Colcon package already exists!')
sys.exit(-1)
print('Creating the colcon package...')
os.system('colcon_create_pkg ' + output_dir)
if not os.path.isdir(output_dir):
print('Colcon package could not be created')
sys.exit(-1)
print('Done!')
else:
output_dir = args.output_dir

print('Create new colcon package for a UUV control configuration')
print('\tRobot name = ' + args.robot_name)
print('\tOutput directory = ' + output_dir)

if os.path.exists(output_dir):
print('Colcon package already exists!')
sys.exit(-1)
print('Creating the colcon package...')
os.system('ros2 pkg create --build-type ament_cmake ' + output_dir)
if not os.path.isdir(output_dir):
print('Colcon package could not be created')
sys.exit(-1)
print('Done!')

# Add installation directory for launch and configuration into CMakeLists.txt of package
pkg_cmake_path = output_dir + "/CMakeLists.txt"
with open(pkg_cmake_path, 'r') as f:
contents = f.readlines()

contents.insert(22, '\n')
contents.insert(23, "install(DIRECTORY launch config\nDESTINATION share/${PROJECT_NAME}\n PATTERN \"*~\"\n EXCLUDE)")
contents.insert(24, '\n')

with open(pkg_cmake_path, "w") as f:
contents = "".join(contents)
f.write(contents)

# Copy templates for thruster manager config
template_path = os.path.join(get_package_share_directory('uuv_assistants'), 'templates', 'thruster_manager_config')

for d in os.listdir(template_path):
Expand Down
180 changes: 180 additions & 0 deletions uuv_assistants/templates/robot_model/launch/upload.launch.py.template
Original file line number Diff line number Diff line change
@@ -0,0 +1,180 @@

from launch import LaunchDescription
from launch.launch_description_sources import AnyLaunchDescriptionSource
from launch_ros.actions import Node, PushRosNamespace
from launch.actions import DeclareLaunchArgument
from launch.actions import GroupAction
from launch.actions import OpaqueFunction
from launch.actions import IncludeLaunchDescription
from launch.substitutions import LaunchConfiguration as Lc

import launch_testing.actions

from ament_index_python.packages import get_package_share_directory

import os
import pathlib
import xacro

from plankton_utils.time import is_sim_time


def to_bool(value: str):
if isinstance(value, bool):
return value
if not isinstance(value, str):
raise ValueError('String to bool, invalid type ' + str(value))

valid = {'true':True, '1':True,
'false':False, '0':False}

if value.lower() in valid:
return valid[value]

raise ValueError('String to bool, invalid value: %s' % value)


# =============================================================================
def launch_setup(context, *args, **kwargs):
# Perform substitutions
debug = Lc('debug').perform(context)
namespace = Lc('namespace').perform(context)
x = Lc('x').perform(context)
y = Lc('y').perform(context)
z = Lc('z').perform(context)
roll = Lc('roll').perform(context)
pitch = Lc('pitch').perform(context)
yaw = Lc('yaw').perform(context)
use_world_ned = Lc('use_ned_frame').perform(context)
is_write_on_disk = Lc('write_file_on_disk').perform(context)

# Request sim time value to the global node
res = is_sim_time(timeout_sec=2, return_param=False, use_subprocess=True)

# Generate robot description xacro path
xacro_file = os.path.join(
get_package_share_directory('$COLCON_PACKAGE'),
'robots',
'$ROBOT_NAME' + '_' + (Lc('mode')).perform(context) + '.xacro'
)

# Check for robot description xacro existence
if not pathlib.Path(xacro_file).exists():
exc = 'Launch file ' + xacro_file + ' does not exist'
raise Exception(exc)

# Build directory for generated robot URDF
path = os.path.join(
get_package_share_directory('$COLCON_PACKAGE'),
'robots',
'generated',
namespace,
)

# If there is no generated robot URDF directory, create it
if not pathlib.Path(path).exists():
try:
# Create directory if required and sub-directory
os.makedirs(path)
except OSError:
print ("Creation of the directory %s failed" % path)

# Generate URDF output path
output = os.path.join(
path,
'robot_description.urdf'
)

# Parse coordinate frame parameter from string and set namespace
mapping = {}
if to_bool(use_world_ned):
mappings={'debug': debug, 'namespace': namespace, 'inertial_reference_frame':'world_ned'}
else:
mappings={'debug': debug, 'namespace': namespace, 'inertial_reference_frame':'world'}

doc = xacro.process(xacro_file, mappings=mappings)

# Save generated robot URDF
if is_write_on_disk:
with open(output, 'w') as file_out:
file_out.write(doc)

# URDF spawner
args=('-gazebo_namespace /gazebo '
'-x %s -y %s -z %s -R %s -P %s -Y %s -entity %s -topic robot_description'
%(x, y, z, roll, pitch, yaw, namespace)).split()

# Urdf spawner. NB: node namespace does not prefix the spawning service,
# as using a leading /
# NB 2: node namespace prefixes the robot_description topic
urdf_spawner = Node(
name = 'urdf_spawner',
package='gazebo_ros',
executable='spawn_entity.py',
output='screen',
parameters=[{'use_sim_time': res}],
arguments=args
)

# A joint state publisher plugin already is started with the model, no need to use the default joint state publisher

# N.B.: alternative way to generate a robot_description string:
# string = str('ros2 run xacro xacro ' + xacro_file + ' debug:=false namespace:=' + namespace + ' inertial_reference_frame:=world')
# cmd = Command(string)
# but it currently yields yaml parsing error due to ":" in the comments of the xacro description files

robot_state_publisher = Node(
name = 'robot_state_publisher',
package='robot_state_publisher',
executable='robot_state_publisher',
output = 'screen',
parameters=[{'use_sim_time': res}, {'robot_description': doc}], # Use subst here
)

# Message to tf
message_to_tf_launch = os.path.join(
get_package_share_directory('uuv_assistants'),
'launch',
'message_to_tf.launch'
)

if not pathlib.Path(message_to_tf_launch).exists():
exc = 'Launch file ' + message_to_tf_launch + ' does not exist'
raise Exception(exc)

launch_args = [('namespace', namespace), ('world_frame', 'world'),
('child_frame_id', '/' + namespace + '/base_link'), ('use_sim_time', str(res).lower()),]
message_to_tf_launch = IncludeLaunchDescription(
AnyLaunchDescriptionSource(message_to_tf_launch), launch_arguments=launch_args)


group = GroupAction([
PushRosNamespace(namespace),
urdf_spawner,
robot_state_publisher,
])


return [group, message_to_tf_launch]

# =============================================================================
def generate_launch_description():
return LaunchDescription([
DeclareLaunchArgument('debug', default_value='0'),

DeclareLaunchArgument('x', default_value='0'),
DeclareLaunchArgument('y', default_value='0'),
DeclareLaunchArgument('z', default_value='-20'),
DeclareLaunchArgument('roll', default_value='0.0'),
DeclareLaunchArgument('pitch', default_value='0.0'),
DeclareLaunchArgument('yaw', default_value='0.0'),

DeclareLaunchArgument('mode', default_value='default'),
DeclareLaunchArgument('namespace', default_value='$ROBOT_NAME'),
DeclareLaunchArgument('use_ned_frame', default_value='false'),
DeclareLaunchArgument('write_file_on_disk', default_value='false'),

# DeclareLaunchArgument('use_sim_time', default_value='true'),
OpaqueFunction(function = launch_setup)
])

Original file line number Diff line number Diff line change
Expand Up @@ -35,7 +35,7 @@
<group>
<push-ros-namespace namespace="$(arg namespace)"/>
<param name="robot_description"
command="$(find xacro)/xacro.py '$(find $CATKIN_PACKAGE)/robots/$(arg mode).xacro' debug:=$(arg debug) namespace:=$(arg namespace)" />
command="$(find xacro)/xacro.py '$(find $COLCON_PACKAGE)/robots/$(arg mode).xacro' debug:=$(arg debug) namespace:=$(arg namespace)" />

<!-- Run a python script to the send a service call to gazebo_ros to spawn a URDF robot -->
<group if="$(arg use_geodetic)">
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -3,8 +3,8 @@
<xacro:arg name="debug" default="0"/>
<xacro:arg name="namespace" default="$ROBOT_NAME"/>
<!-- Include the ROV macro file -->
<xacro:include filename="$(find $CATKIN_PACKAGE)/urdf/base.xacro"/>
<xacro:include filename="$(find $CATKIN_PACKAGE)/urdf/gazebo.xacro"/>
<xacro:include filename="$(find $COLCON_PACKAGE)/urdf/base.xacro"/>
<xacro:include filename="$(find $COLCON_PACKAGE)/urdf/gazebo.xacro"/>

<!-- Create the $ROBOT_NAME -->
<xacro:$ROBOT_NAME_base namespace="$(arg namespace)">
Expand Down
10 changes: 5 additions & 5 deletions uuv_assistants/templates/robot_model/urdf/base.xacro.template
Original file line number Diff line number Diff line change
Expand Up @@ -7,7 +7,7 @@
<!-- Loading the UUV simulator ROS plugin macros -->
<xacro:include filename="$(find uuv_gazebo_ros_plugins)/urdf/snippets.xacro"/>
<!-- Loading vehicle's specific macros -->
<xacro:include filename="$(find $CATKIN_PACKAGE)/urdf/snippets.xacro"/>
<xacro:include filename="$(find $COLCON_PACKAGE)/urdf/snippets.xacro"/>

<!--
Vehicle's parameters (remember to enter the model parameters below)
Expand All @@ -25,12 +25,12 @@
the mesh pose will have to be corrected below in the <visual> block.
Open the meshes for the RexROV vehicle in Blender to see an example on the mesh placement.
-->
<xacro:property name="visual_mesh_file" value="file://$(find $CATKIN_PACKAGE)/meshes/vehicle.dae"/>
<xacro:property name="visual_mesh_file" value="file://$(find $COLCON_PACKAGE)/meshes/vehicle.dae"/>

<!-- Collision geometry mesh, usually in STL format (it is recommended to keep
this geometry as simple as possible to improve the performance the physics engine
regarding the computation of collision forces) -->
<xacro:property name="collision_mesh_file" value="file://$(find $CATKIN_PACKAGE)/meshes/vehicle.stl"/>
<xacro:property name="collision_mesh_file" value="file://$(find $COLCON_PACKAGE)/meshes/vehicle.stl"/>

<!-- Vehicle macro -->
<xacro:macro name="$ROBOT_NAME_base" params="namespace *gazebo">
Expand Down Expand Up @@ -72,10 +72,10 @@
<xacro:insert_block name="gazebo"/>

<!-- Include the thruster modules -->
<xacro:include filename="$(find $CATKIN_PACKAGE)/urdf/actuators.xacro"/>
<xacro:include filename="$(find $COLCON_PACKAGE)/urdf/actuators.xacro"/>

<!-- Include the sensor modules -->
<xacro:include filename="$(find $CATKIN_PACKAGE)/urdf/sensors.xacro"/>
<xacro:include filename="$(find $COLCON_PACKAGE)/urdf/sensors.xacro"/>

</xacro:macro>

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -8,7 +8,7 @@
<!-- Provide the propeller mesh in a separate file with the rotation axis
over propeller's frame X-axis in DAE (Collada) or STL format.
-->
<xacro:property name="prop_mesh_file" value="file://$(find $CATKIN_PACKAGE)/meshes/propeller.dae"/>
<xacro:property name="prop_mesh_file" value="file://$(find $COLCON_PACKAGE)/meshes/propeller.dae"/>

<!--
Thruster macro with integration of joint and link. The thrusters should
Expand Down Expand Up @@ -194,7 +194,7 @@
-->

<!-- A separate mesh for the fin should be stored in the meshes folder -->
<xacro:property name="fin_mesh_file" value="file://$(find $CATKIN_PACKAGE)/meshes/fin.dae"/>
<xacro:property name="fin_mesh_file" value="file://$(find $COLCON_PACKAGE)/meshes/fin.dae"/>

<!-- Fin joint limits -->
<xacro:property name="fin_min_joint_limit" value="${0.0 * d2r}"/>
Expand Down
Loading