From d77ffd96c8741af3f8f02f6aff6d21c633c52f83 Mon Sep 17 00:00:00 2001 From: devbharat Date: Sat, 24 Oct 2015 03:13:46 +0200 Subject: [PATCH 1/5] Autogenerating iris.sdf from xacro files using python script in cmake file. Still need to set paths to meshes correctly. --- CMakeLists.txt | 11 + iris.sdf | 22 +- .../urdf/component_snippets.xacro | 454 +++++++++++ rotors_description/urdf/iris.xacro | 149 ++++ rotors_description/urdf/iris_base.urdf | 355 +++++++++ rotors_description/urdf/iris_base.xacro | 101 +++ rotors_description/urdf/multirotor_base.xacro | 130 ++++ xacro.py | 709 ++++++++++++++++++ 8 files changed, 1920 insertions(+), 11 deletions(-) create mode 100644 rotors_description/urdf/component_snippets.xacro create mode 100644 rotors_description/urdf/iris.xacro create mode 100644 rotors_description/urdf/iris_base.urdf create mode 100644 rotors_description/urdf/iris_base.xacro create mode 100644 rotors_description/urdf/multirotor_base.xacro create mode 100755 xacro.py diff --git a/CMakeLists.txt b/CMakeLists.txt index 9087ec6877..7d67ee822d 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -19,6 +19,17 @@ include_directories( ${CMAKE_CURRENT_BINARY_DIR}/msgs ) +set(URDF_DIR "${CMAKE_SOURCE_DIR}/rotors_description/urdf") +set(enable_mavlink_interface "true") +set(enable_ground_truth "false") +set(enable_logging "false") +add_custom_target(submodule_cleanly + WORKING_DIRECTORY ${CMAKE_SOURCE_DIR} + COMMAND rm iris.sdf + COMMAND python xacro.py -o ${URDF_DIR}/iris_base.urdf ${URDF_DIR}/iris_base.xacro enable_mavlink_interface:=${enable_mavlink_interface} enable_ground_truth:=${enable_ground_truth} enable_logging:=${enable_logging} + COMMAND gz sdf -p ${URDF_DIR}/iris_base.urdf >> iris.sdf + ) + link_directories(${GAZEBO_LIBRARY_DIRS} ${CMAKE_CURRENT_BINARY_DIR}/msgs) add_subdirectory(msgs) diff --git a/iris.sdf b/iris.sdf index 1eb9ee6f1f..50ca78d277 100644 --- a/iris.sdf +++ b/iris.sdf @@ -35,7 +35,7 @@ 1 1 1 - model://iris/meshes/iris.dae + model://rotors_description/meshes/iris.dae @@ -118,7 +118,7 @@ 1 1 1 - model://iris/meshes/iris_prop_ccw.dae + model://rotors_description/meshes/iris_prop_ccw.dae @@ -184,7 +184,7 @@ 1 1 1 - model://iris/meshes/iris_prop_ccw.dae + model://rotors_description/meshes/iris_prop_ccw.dae @@ -250,7 +250,7 @@ 1 1 1 - model://iris/meshes/iris_prop_cw.dae + model://rotors_description/meshes/iris_prop_cw.dae @@ -316,7 +316,7 @@ 1 1 1 - model://iris/meshes/iris_prop_cw.dae + model://rotors_description/meshes/iris_prop_cw.dae @@ -419,6 +419,12 @@ motor_speed/3 10 + + iris + imu + gazebo/command/motor_speed + + 0 iris iris/imu_link @@ -432,11 +438,5 @@ 300.0 0.196 - - iris - imu - gazebo/command/motor_speed - - 0 diff --git a/rotors_description/urdf/component_snippets.xacro b/rotors_description/urdf/component_snippets.xacro new file mode 100644 index 0000000000..5f93a1cce9 --- /dev/null +++ b/rotors_description/urdf/component_snippets.xacro @@ -0,0 +1,454 @@ + + + + + + + + + ${namespace} + ${bag_file} + base_link + base_link + command/attitude + command/attitude + ${rotor_velocity_slowdown_sim} + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + ${frame_rate} + + ${horizontal_fov} + + ${image_width} + ${image_height} + ${image_format} + + + ${min_distance} + ${max_distance} + + + gaussian + + ${noise_mean} + ${noise_stddev} + + + + ${namespace} + true + ${frame_rate} + camera_${camera_suffix} + image_raw + camera_info + camera_${camera_suffix}_link + 0.0 + 0.0 + 0.0 + 0.0 + 0.0 + 0.0 + + + + + + + + + + ${namespace} + command/attitude + command/rate + command/motor_speed + ${imu_sub_topic} + gazebo/command/motor_speed + + + + + + + + + ${namespace} + ${imu_sub_topic} + gazebo/command/motor_speed + + + + + + + + + + + + + + + + + + + + + + + + + ${namespace} + ${namespace}/imu${imu_suffix}_link + ${imu_topic} + ${gyroscope_noise_density} + ${gyroscopoe_random_walk} + ${gyroscope_bias_correlation_time} + ${gyroscope_turn_on_bias_sigma} + ${accelerometer_noise_density} + ${accelerometer_random_walk} + ${accelerometer_bias_correlation_time} + ${accelerometer_turn_on_bias_sigma} + + + + + + + + + + + + + + + + + + + + + + + ${namespace}/odometry_sensor${odometry_sensor_suffix}_link + ${namespace} + ${pose_topic} + ${pose_with_covariance_topic} + ${position_topic} + ${transform_topic} + ${odometry_topic} + ${parent_frame_id} + ${measurement_divisor} + ${measurement_delay} + ${unknown_delay} + ${noise_normal_position} + ${noise_normal_quaternion} + ${noise_normal_linear_velocity} + ${noise_normal_angular_velocity} + ${noise_uniform_position} + ${noise_uniform_quaternion} + ${noise_uniform_linear_velocity} + ${noise_uniform_angular_velocity} + + package://rotors_gazebo/resource/${odometry_map} + ${image_scale} + + + + + + + + + + base_link + base_link + ${namespace} + ${xyz_offset} + ${wind_direction} + ${wind_force_mean} + ${wind_gust_direction} + ${wind_gust_duration} + ${wind_gust_start} + ${wind_gust_force_mean} + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + true + ${frame_rate} + + 2 + + L8 + 640 + 480 + + + 0.01 + 100 + + + + ${namespace} + true + 0.11 + ${frame_rate} + camera_${camera_suffix} + camera/image_raw + camera/camera_info + depth/disparity + depth/camera_info + depth/points + camera_${camera_suffix}_optical_center_link + 0.5 + 0.0 + 0.0 + 0.0 + 0.0 + 0.0 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/rotors_description/urdf/iris.xacro b/rotors_description/urdf/iris.xacro new file mode 100644 index 0000000000..23f88276b9 --- /dev/null +++ b/rotors_description/urdf/iris.xacro @@ -0,0 +1,149 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/rotors_description/urdf/iris_base.urdf b/rotors_description/urdf/iris_base.urdf new file mode 100644 index 0000000000..c590f94a14 --- /dev/null +++ b/rotors_description/urdf/iris_base.urdf @@ -0,0 +1,355 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + iris + base_link + 10 + + + + Gazebo/DarkGrey + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + iris + rotor_0_joint + rotor_0 + ccw + 0.0125 + 0.025 + 838 + 8.54858e-06 + 0.016 + gazebo/command/motor_speed + 0 + 8.06428e-05 + 1e-06 + motor_speed/0 + 10 + + + + Gazebo/Blue + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + iris + rotor_1_joint + rotor_1 + ccw + 0.0125 + 0.025 + 838 + 8.54858e-06 + 0.016 + gazebo/command/motor_speed + 1 + 8.06428e-05 + 1e-06 + motor_speed/1 + 10 + + + + Gazebo/DarkGrey + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + iris + rotor_2_joint + rotor_2 + cw + 0.0125 + 0.025 + 838 + 8.54858e-06 + 0.016 + gazebo/command/motor_speed + 2 + 8.06428e-05 + 1e-06 + motor_speed/2 + 10 + + + + Gazebo/Blue + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + iris + rotor_3_joint + rotor_3 + cw + 0.0125 + 0.025 + 838 + 8.54858e-06 + 0.016 + gazebo/command/motor_speed + 3 + 8.06428e-05 + 1e-06 + motor_speed/3 + 10 + + + + Gazebo/DarkGrey + + + + + iris + imu + gazebo/command/motor_speed + + + + + + + + + + + + + + + + + + + + + + + iris + + iris/imu_link + + imu + + 0.0003394 + + 3.8785e-05 + + 1000.0 + + 0.0087 + + 0.004 + + 0.006 + + 300.0 + + 0.196 + + + + diff --git a/rotors_description/urdf/iris_base.xacro b/rotors_description/urdf/iris_base.xacro new file mode 100644 index 0000000000..44a2d0fbea --- /dev/null +++ b/rotors_description/urdf/iris_base.xacro @@ -0,0 +1,101 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/rotors_description/urdf/multirotor_base.xacro b/rotors_description/urdf/multirotor_base.xacro new file mode 100644 index 0000000000..40be30f01a --- /dev/null +++ b/rotors_description/urdf/multirotor_base.xacro @@ -0,0 +1,130 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + ${robot_namespace} + base_link + ${rotor_velocity_slowdown_sim} + + + + + Gazebo/${color} + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + ${robot_namespace} + rotor_${motor_number}_joint + rotor_${motor_number} + ${direction} + ${time_constant_up} + ${time_constant_down} + ${max_rot_velocity} + ${motor_constant} + ${moment_constant} + gazebo/command/motor_speed + ${motor_number} + ${rotor_drag_coefficient} + ${rolling_moment_coefficient} + motor_speed/${motor_number} + ${rotor_velocity_slowdown_sim} + + + + Gazebo/${color} + + + diff --git a/xacro.py b/xacro.py new file mode 100755 index 0000000000..08a3d493da --- /dev/null +++ b/xacro.py @@ -0,0 +1,709 @@ +# Copyright (c) 2013, Willow Garage, Inc. +# Copyright (c) 2014, Open Source Robotics Foundation, Inc. +# All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions are met: +# +# * Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# * Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in the +# documentation and/or other materials provided with the distribution. +# * Neither the name of the Open Source Robotics Foundation, Inc. +# nor the names of its contributors may be used to endorse or promote +# products derived from this software without specific prior +# written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. + +# Author: Stuart Glaser +# Maintainer: William Woodall + +from __future__ import print_function + +import getopt +import glob +import os +import re +import string +import sys +import xml + +from xml.dom.minidom import parse + +from roslaunch import substitution_args +from rosgraph.names import load_mappings + +try: + _basestr = basestring +except NameError: + _basestr = str + +# Dictionary of subtitution args +substitution_args_context = {} + + +class XacroException(Exception): + pass + + +def isnumber(x): + return hasattr(x, '__int__') + + +def eval_extension(str): + return substitution_args.resolve_args(str, context=substitution_args_context, resolve_anon=False) + + +# Better pretty printing of xml +# Taken from http://ronrothman.com/public/leftbraned/xml-dom-minidom-toprettyxml-and-silly-whitespace/ +def fixed_writexml(self, writer, indent="", addindent="", newl=""): + # indent = current indentation + # addindent = indentation to add to higher levels + # newl = newline string + writer.write(indent + "<" + self.tagName) + + attrs = self._get_attributes() + a_names = list(attrs.keys()) + a_names.sort() + + for a_name in a_names: + writer.write(" %s=\"" % a_name) + xml.dom.minidom._write_data(writer, attrs[a_name].value) + writer.write("\"") + if self.childNodes: + if len(self.childNodes) == 1 \ + and self.childNodes[0].nodeType == xml.dom.minidom.Node.TEXT_NODE: + writer.write(">") + self.childNodes[0].writexml(writer, "", "", "") + writer.write("%s" % (self.tagName, newl)) + return + writer.write(">%s" % (newl)) + for node in self.childNodes: + # skip whitespace-only text nodes + if node.nodeType == xml.dom.minidom.Node.TEXT_NODE and \ + not node.data.strip(): + continue + node.writexml(writer, indent + addindent, addindent, newl) + writer.write("%s%s" % (indent, self.tagName, newl)) + else: + writer.write("/>%s" % (newl)) +# replace minidom's function with ours +xml.dom.minidom.Element.writexml = fixed_writexml + + +class Table: + def __init__(self, parent=None): + self.parent = parent + self.table = {} + + def __getitem__(self, key): + if key in self.table: + return self.table[key] + elif self.parent: + return self.parent[key] + else: + raise KeyError(key) + + def __setitem__(self, key, value): + self.table[key] = value + + def __contains__(self, key): + return \ + key in self.table or \ + (self.parent and key in self.parent) + + +class QuickLexer(object): + def __init__(self, **res): + self.str = "" + self.top = None + self.res = [] + for k, v in res.items(): + self.__setattr__(k, len(self.res)) + self.res.append(v) + + def lex(self, str): + self.str = str + self.top = None + self.next() + + def peek(self): + return self.top + + def next(self): + result = self.top + self.top = None + for i in range(len(self.res)): + m = re.match(self.res[i], self.str) + if m: + self.top = (i, m.group(0)) + self.str = self.str[m.end():] + break + return result + + +def first_child_element(elt): + c = elt.firstChild + while c: + if c.nodeType == xml.dom.Node.ELEMENT_NODE: + return c + c = c.nextSibling + return None + + +def next_sibling_element(elt): + c = elt.nextSibling + while c: + if c.nodeType == xml.dom.Node.ELEMENT_NODE: + return c + c = c.nextSibling + return None + + +# Pre-order traversal of the elements +def next_element(elt): + child = first_child_element(elt) + if child: + return child + while elt and elt.nodeType == xml.dom.Node.ELEMENT_NODE: + next = next_sibling_element(elt) + if next: + return next + elt = elt.parentNode + return None + + +# Pre-order traversal of all the nodes +def next_node(node): + if node.firstChild: + return node.firstChild + while node: + if node.nextSibling: + return node.nextSibling + node = node.parentNode + return None + + +def child_nodes(elt): + c = elt.firstChild + while c: + yield c + c = c.nextSibling + +all_includes = [] + +# Deprecated message for tags that don't have prepended: +deprecated_include_msg = """DEPRECATED IN HYDRO: + The tag should be prepended with 'xacro' if that is the intended use + of it, such as . Use the following script to fix incorrect + xacro includes: + sed -i 's/ extensions + is_include = False + if elt.tagName == 'xacro:include' or elt.tagName == 'include': + + is_include = True + # Temporary fix for ROS Hydro and the xacro include scope problem + if elt.tagName == 'include': + + # check if there is any element within the tag. mostly we are concerned + # with Gazebo's element, but it could be anything. also, make sure the child + # nodes aren't just a single Text node, which is still considered a deprecated + # instance + if elt.childNodes and not (len(elt.childNodes) == 1 and + elt.childNodes[0].nodeType == elt.TEXT_NODE): + # this is not intended to be a xacro element, so we can ignore it + is_include = False + else: + # throw a deprecated warning + print(deprecated_include_msg, file=sys.stderr) + + # Process current element depending on previous conditions + if is_include: + filename_spec = eval_text(elt.getAttribute('filename'), {}) + if not os.path.isabs(filename_spec): + filename_spec = os.path.join(base_dir, filename_spec) + + if re.search('[*[?]+', filename_spec): + # Globbing behaviour + filenames = sorted(glob.glob(filename_spec)) + if len(filenames) == 0: + print(include_no_matches_msg.format(filename_spec), file=sys.stderr) + else: + # Default behaviour + filenames = [filename_spec] + + for filename in filenames: + global all_includes + all_includes.append(filename) + try: + with open(filename) as f: + try: + included = parse(f) + except Exception as e: + raise XacroException( + "included file \"%s\" generated an error during XML parsing: %s" + % (filename, str(e))) + except IOError as e: + raise XacroException("included file \"%s\" could not be opened: %s" % (filename, str(e))) + + # Replaces the include tag with the elements of the included file + for c in child_nodes(included.documentElement): + elt.parentNode.insertBefore(c.cloneNode(deep=True), elt) + + # Grabs all the declared namespaces of the included document + for name, value in included.documentElement.attributes.items(): + if name.startswith('xmlns:'): + namespaces[name] = value + + elt.parentNode.removeChild(elt) + elt = None + else: + previous = elt + + elt = next_element(previous) + + # Makes sure the final document declares all the namespaces of the included documents. + for k, v in namespaces.items(): + doc.documentElement.setAttribute(k, v) + + +# Returns a dictionary: { macro_name => macro_xml_block } +def grab_macros(doc): + macros = {} + + previous = doc.documentElement + elt = next_element(previous) + while elt: + if elt.tagName == 'macro' or elt.tagName == 'xacro:macro': + name = elt.getAttribute('name') + + macros[name] = elt + macros['xacro:' + name] = elt + + elt.parentNode.removeChild(elt) + elt = None + else: + previous = elt + + elt = next_element(previous) + return macros + + +# Returns a Table of the properties +def grab_properties(doc): + table = Table() + + previous = doc.documentElement + elt = next_element(previous) + while elt: + if elt.tagName == 'property' or elt.tagName == 'xacro:property': + name = elt.getAttribute('name') + value = None + + if elt.hasAttribute('value'): + value = elt.getAttribute('value') + else: + name = '**' + name + value = elt # debug + + bad = string.whitespace + "${}" + has_bad = False + for b in bad: + if b in name: + has_bad = True + break + + if has_bad: + sys.stderr.write('Property names may not have whitespace, ' + + '"{", "}", or "$" : "' + name + '"') + else: + table[name] = value + + elt.parentNode.removeChild(elt) + elt = None + else: + previous = elt + + elt = next_element(previous) + return table + + +def eat_ignore(lex): + while lex.peek() and lex.peek()[0] == lex.IGNORE: + lex.next() + + +def eval_lit(lex, symbols): + eat_ignore(lex) + if lex.peek()[0] == lex.NUMBER: + return float(lex.next()[1]) + if lex.peek()[0] == lex.SYMBOL: + try: + key = lex.next()[1] + value = symbols[key] + except KeyError as ex: + raise XacroException("Property wasn't defined: %s" % str(ex)) + if not (isnumber(value) or isinstance(value, _basestr)): + if value is None: + raise XacroException("Property %s recursively used" % key) + raise XacroException("WTF2") + try: + return int(value) + except: + try: + return float(value) + except: + # prevent infinite recursion + symbols[key] = None + result = eval_text(value, symbols) + # restore old entry + symbols[key] = value + return result + raise XacroException("Bad literal") + + +def eval_factor(lex, symbols): + eat_ignore(lex) + + neg = 1 + if lex.peek()[1] == '-': + lex.next() + neg = -1 + + if lex.peek()[0] in [lex.NUMBER, lex.SYMBOL]: + return neg * eval_lit(lex, symbols) + if lex.peek()[0] == lex.LPAREN: + lex.next() + eat_ignore(lex) + result = eval_expr(lex, symbols) + eat_ignore(lex) + if lex.next()[0] != lex.RPAREN: + raise XacroException("Unmatched left paren") + eat_ignore(lex) + return neg * result + + raise XacroException("Misplaced operator") + + +def eval_term(lex, symbols): + eat_ignore(lex) + + result = 0 + if lex.peek()[0] in [lex.NUMBER, lex.SYMBOL, lex.LPAREN] \ + or lex.peek()[1] == '-': + result = eval_factor(lex, symbols) + + eat_ignore(lex) + while lex.peek() and lex.peek()[1] in ['*', '/']: + op = lex.next()[1] + n = eval_factor(lex, symbols) + + if op == '*': + result = float(result) * float(n) + elif op == '/': + result = float(result) / float(n) + else: + raise XacroException("WTF") + eat_ignore(lex) + return result + + +def eval_expr(lex, symbols): + eat_ignore(lex) + + op = None + if lex.peek()[0] == lex.OP: + op = lex.next()[1] + if not op in ['+', '-']: + raise XacroException("Invalid operation. Must be '+' or '-'") + + result = eval_term(lex, symbols) + if op == '-': + result = -float(result) + + eat_ignore(lex) + while lex.peek() and lex.peek()[1] in ['+', '-']: + op = lex.next()[1] + n = eval_term(lex, symbols) + + if op == '+': + result = float(result) + float(n) + if op == '-': + result = float(result) - float(n) + eat_ignore(lex) + return result + + +def eval_text(text, symbols): + def handle_expr(s): + lex = QuickLexer(IGNORE=r"\s+", + NUMBER=r"(\d+(\.\d*)?|\.\d+)([eE][-+]?\d+)?", + SYMBOL=r"[a-zA-Z_]\w*", + OP=r"[\+\-\*/^]", + LPAREN=r"\(", + RPAREN=r"\)") + lex.lex(s) + return eval_expr(lex, symbols) + + def handle_extension(s): + return eval_extension("$(%s)" % s) + + results = [] + lex = QuickLexer(DOLLAR_DOLLAR_BRACE=r"\$\$+\{", + EXPR=r"\$\{[^\}]*\}", + EXTENSION=r"\$\([^\)]*\)", + TEXT=r"([^\$]|\$[^{(]|\$$)+") + lex.lex(text) + while lex.peek(): + if lex.peek()[0] == lex.EXPR: + results.append(handle_expr(lex.next()[1][2:-1])) + elif lex.peek()[0] == lex.EXTENSION: + results.append(handle_extension(lex.next()[1][2:-1])) + elif lex.peek()[0] == lex.TEXT: + results.append(lex.next()[1]) + elif lex.peek()[0] == lex.DOLLAR_DOLLAR_BRACE: + results.append(lex.next()[1][1:]) + return ''.join(map(str, results)) + + +# Expands macros, replaces properties, and evaluates expressions +def eval_all(root, macros, symbols): + # Evaluates the attributes for the root node + for at in root.attributes.items(): + result = eval_text(at[1], symbols) + root.setAttribute(at[0], result) + + previous = root + node = next_node(previous) + while node: + if node.nodeType == xml.dom.Node.ELEMENT_NODE: + if node.tagName in macros: + body = macros[node.tagName].cloneNode(deep=True) + params = body.getAttribute('params').split() + + # Parse default values for any parameters + defaultmap = {} + for param in params[:]: + splitParam = param.split(':=') + + if len(splitParam) == 2: + defaultmap[splitParam[0]] = splitParam[1] + params.remove(param) + params.append(splitParam[0]) + + elif len(splitParam) != 1: + raise XacroException("Invalid parameter definition") + + # Expands the macro + scoped = Table(symbols) + for name, value in node.attributes.items(): + if not name in params: + raise XacroException("Invalid parameter \"%s\" while expanding macro \"%s\"" % + (str(name), str(node.tagName))) + params.remove(name) + scoped[name] = eval_text(value, symbols) + + # Pulls out the block arguments, in order + cloned = node.cloneNode(deep=True) + eval_all(cloned, macros, symbols) + block = cloned.firstChild + for param in params[:]: + if param[0] == '*': + while block and block.nodeType != xml.dom.Node.ELEMENT_NODE: + block = block.nextSibling + if not block: + raise XacroException("Not enough blocks while evaluating macro %s" % str(node.tagName)) + params.remove(param) + scoped[param] = block + block = block.nextSibling + + # Try to load defaults for any remaining non-block parameters + for param in params[:]: + if param[0] != '*' and param in defaultmap: + scoped[param] = defaultmap[param] + params.remove(param) + + if params: + raise XacroException("Parameters [%s] were not set for macro %s" % + (",".join(params), str(node.tagName))) + eval_all(body, macros, scoped) + + # Replaces the macro node with the expansion + for e in list(child_nodes(body)): # Ew + node.parentNode.insertBefore(e, node) + node.parentNode.removeChild(node) + + node = None + elif node.tagName == 'xacro:arg': + name = node.getAttribute('name') + if not name: + raise XacroException("Argument name missing") + default = node.getAttribute('default') + if default and name not in substitution_args_context['arg']: + substitution_args_context['arg'][name] = default + + node.parentNode.removeChild(node) + node = None + + elif node.tagName == 'insert_block' or node.tagName == 'xacro:insert_block': + name = node.getAttribute('name') + + if ("**" + name) in symbols: + # Multi-block + block = symbols['**' + name] + + for e in list(child_nodes(block)): + node.parentNode.insertBefore(e.cloneNode(deep=True), node) + node.parentNode.removeChild(node) + elif ("*" + name) in symbols: + # Single block + block = symbols['*' + name] + + node.parentNode.insertBefore(block.cloneNode(deep=True), node) + node.parentNode.removeChild(node) + else: + raise XacroException("Block \"%s\" was never declared" % name) + + node = None + elif node.tagName in ['if', 'xacro:if', 'unless', 'xacro:unless']: + value = eval_text(node.getAttribute('value'), symbols) + try: + if value == 'true': keep = True + elif value == 'false': keep = False + else: keep = float(value) + except ValueError: + raise XacroException("Xacro conditional evaluated to \"%s\". Acceptable evaluations are one of [\"1\",\"true\",\"0\",\"false\"]" % value) + if node.tagName in ['unless', 'xacro:unless']: keep = not keep + if keep: + for e in list(child_nodes(node)): + node.parentNode.insertBefore(e.cloneNode(deep=True), node) + + node.parentNode.removeChild(node) + else: + # Evals the attributes + for at in node.attributes.items(): + result = eval_text(at[1], symbols) + node.setAttribute(at[0], result) + previous = node + elif node.nodeType == xml.dom.Node.TEXT_NODE: + node.data = eval_text(node.data, symbols) + previous = node + else: + previous = node + + node = next_node(previous) + return macros + + +# Expands everything except includes +def eval_self_contained(doc): + macros = grab_macros(doc) + symbols = grab_properties(doc) + eval_all(doc.documentElement, macros, symbols) + + +def print_usage(exit_code=0): + print("Usage: %s [-o ] " % 'xacro.py') + print(" %s --deps Prints dependencies" % 'xacro.py') + print(" %s --includes Only evalutes includes" % 'xacro.py') + sys.exit(exit_code) + + +def set_substitution_args_context(context={}): + substitution_args_context['arg'] = context + +def open_output(output_filename): + if output_filename is None: + return sys.stdout + else: + return open(output_filename, 'w') + +def main(): + try: + opts, args = getopt.gnu_getopt(sys.argv[1:], "ho:", ['deps', 'includes']) + except getopt.GetoptError as err: + print(str(err)) + print_usage(2) + + just_deps = False + just_includes = False + + output_filename = None + for o, a in opts: + if o == '-h': + print_usage(0) + elif o == '-o': + output_filename = a + elif o == '--deps': + just_deps = True + elif o == '--includes': + just_includes = True + + if len(args) < 1: + print("No input given") + print_usage(2) + + # Process substitution args + set_substitution_args_context(load_mappings(sys.argv)) + + f = open(args[0]) + doc = None + try: + doc = parse(f) + except xml.parsers.expat.ExpatError: + sys.stderr.write("Expat parsing error. Check that:\n") + sys.stderr.write(" - Your XML is correctly formed\n") + sys.stderr.write(" - You have the xacro xmlns declaration: " + + "xmlns:xacro=\"http://www.ros.org/wiki/xacro\"\n") + sys.stderr.write("\n") + raise + finally: + f.close() + + process_includes(doc, os.path.dirname(args[0])) + if just_deps: + for inc in all_includes: + sys.stdout.write(inc + " ") + sys.stdout.write("\n") + elif just_includes: + doc.writexml(open_output(output_filename)) + print() + else: + eval_self_contained(doc) + banner = [xml.dom.minidom.Comment(c) for c in + [" %s " % ('=' * 83), + " | This document was autogenerated by xacro from %-30s | " % args[0], + " | EDITING THIS FILE BY HAND IS NOT RECOMMENDED %-30s | " % "", + " %s " % ('=' * 83)]] + first = doc.firstChild + for comment in banner: + doc.insertBefore(comment, first) + + open_output(output_filename).write(doc.toprettyxml(indent=' ')) + print() + +main() From 2cebfbb7375abbadc0ff42c37fe605845572e9fb Mon Sep 17 00:00:00 2001 From: devbharat Date: Sat, 24 Oct 2015 04:01:56 +0200 Subject: [PATCH 2/5] Moved rotors description into models directory. Need to untrack sdf file. Run tested on sitl. Need to change motor_model time constant parameter back to 0.16 --- CMakeLists.txt | 8 ++++---- models/iris/iris.sdf | 8 ++++---- models/{iris => rotors_description}/meshes/iris.dae | 0 .../{iris => rotors_description}/meshes/iris_prop_ccw.dae | 0 .../{iris => rotors_description}/meshes/iris_prop_cw.dae | 0 .../{iris => rotors_description}/meshes/propeller_ccw.dae | 0 .../{iris => rotors_description}/meshes/propeller_cw.dae | 0 .../rotors_description}/urdf/component_snippets.xacro | 0 .../rotors_description}/urdf/iris.xacro | 0 .../rotors_description}/urdf/iris_base.urdf | 2 +- .../rotors_description}/urdf/iris_base.xacro | 0 .../rotors_description}/urdf/multirotor_base.xacro | 0 12 files changed, 9 insertions(+), 9 deletions(-) rename models/{iris => rotors_description}/meshes/iris.dae (100%) rename models/{iris => rotors_description}/meshes/iris_prop_ccw.dae (100%) rename models/{iris => rotors_description}/meshes/iris_prop_cw.dae (100%) rename models/{iris => rotors_description}/meshes/propeller_ccw.dae (100%) rename models/{iris => rotors_description}/meshes/propeller_cw.dae (100%) rename {rotors_description => models/rotors_description}/urdf/component_snippets.xacro (100%) rename {rotors_description => models/rotors_description}/urdf/iris.xacro (100%) rename {rotors_description => models/rotors_description}/urdf/iris_base.urdf (99%) rename {rotors_description => models/rotors_description}/urdf/iris_base.xacro (100%) rename {rotors_description => models/rotors_description}/urdf/multirotor_base.xacro (100%) diff --git a/CMakeLists.txt b/CMakeLists.txt index 02dede20da..586960ee52 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -19,15 +19,15 @@ include_directories( ${CMAKE_CURRENT_BINARY_DIR}/msgs ) -set(URDF_DIR "${CMAKE_SOURCE_DIR}/rotors_description/urdf") +set(URDF_DIR "${CMAKE_SOURCE_DIR}/models/rotors_description/urdf") set(enable_mavlink_interface "true") set(enable_ground_truth "false") set(enable_logging "false") -add_custom_target(submodule_cleanly +add_custom_target(sdf WORKING_DIRECTORY ${CMAKE_SOURCE_DIR} - COMMAND rm iris.sdf + COMMAND rm -f ${CMAKE_SOURCE_DIR}/models/iris/iris.sdf COMMAND python xacro.py -o ${URDF_DIR}/iris_base.urdf ${URDF_DIR}/iris_base.xacro enable_mavlink_interface:=${enable_mavlink_interface} enable_ground_truth:=${enable_ground_truth} enable_logging:=${enable_logging} - COMMAND gz sdf -p ${URDF_DIR}/iris_base.urdf >> iris.sdf + COMMAND gz sdf -p ${URDF_DIR}/iris_base.urdf >> ${CMAKE_SOURCE_DIR}/models/iris/iris.sdf ) link_directories(${GAZEBO_LIBRARY_DIRS} ${CMAKE_CURRENT_BINARY_DIR}/msgs) diff --git a/models/iris/iris.sdf b/models/iris/iris.sdf index 7f88877c82..50ca78d277 100644 --- a/models/iris/iris.sdf +++ b/models/iris/iris.sdf @@ -360,7 +360,7 @@ 0.025 838 8.54858e-06 - 0.16 + 0.016 gazebo/command/motor_speed 0 8.06428e-05 @@ -377,7 +377,7 @@ 0.025 838 8.54858e-06 - 0.16 + 0.016 gazebo/command/motor_speed 1 8.06428e-05 @@ -394,7 +394,7 @@ 0.025 838 8.54858e-06 - 0.16 + 0.016 gazebo/command/motor_speed 2 8.06428e-05 @@ -411,7 +411,7 @@ 0.025 838 8.54858e-06 - 0.16 + 0.016 gazebo/command/motor_speed 3 8.06428e-05 diff --git a/models/iris/meshes/iris.dae b/models/rotors_description/meshes/iris.dae similarity index 100% rename from models/iris/meshes/iris.dae rename to models/rotors_description/meshes/iris.dae diff --git a/models/iris/meshes/iris_prop_ccw.dae b/models/rotors_description/meshes/iris_prop_ccw.dae similarity index 100% rename from models/iris/meshes/iris_prop_ccw.dae rename to models/rotors_description/meshes/iris_prop_ccw.dae diff --git a/models/iris/meshes/iris_prop_cw.dae b/models/rotors_description/meshes/iris_prop_cw.dae similarity index 100% rename from models/iris/meshes/iris_prop_cw.dae rename to models/rotors_description/meshes/iris_prop_cw.dae diff --git a/models/iris/meshes/propeller_ccw.dae b/models/rotors_description/meshes/propeller_ccw.dae similarity index 100% rename from models/iris/meshes/propeller_ccw.dae rename to models/rotors_description/meshes/propeller_ccw.dae diff --git a/models/iris/meshes/propeller_cw.dae b/models/rotors_description/meshes/propeller_cw.dae similarity index 100% rename from models/iris/meshes/propeller_cw.dae rename to models/rotors_description/meshes/propeller_cw.dae diff --git a/rotors_description/urdf/component_snippets.xacro b/models/rotors_description/urdf/component_snippets.xacro similarity index 100% rename from rotors_description/urdf/component_snippets.xacro rename to models/rotors_description/urdf/component_snippets.xacro diff --git a/rotors_description/urdf/iris.xacro b/models/rotors_description/urdf/iris.xacro similarity index 100% rename from rotors_description/urdf/iris.xacro rename to models/rotors_description/urdf/iris.xacro diff --git a/rotors_description/urdf/iris_base.urdf b/models/rotors_description/urdf/iris_base.urdf similarity index 99% rename from rotors_description/urdf/iris_base.urdf rename to models/rotors_description/urdf/iris_base.urdf index c590f94a14..fe96aaee0d 100644 --- a/rotors_description/urdf/iris_base.urdf +++ b/models/rotors_description/urdf/iris_base.urdf @@ -1,6 +1,6 @@ - + diff --git a/rotors_description/urdf/iris_base.xacro b/models/rotors_description/urdf/iris_base.xacro similarity index 100% rename from rotors_description/urdf/iris_base.xacro rename to models/rotors_description/urdf/iris_base.xacro diff --git a/rotors_description/urdf/multirotor_base.xacro b/models/rotors_description/urdf/multirotor_base.xacro similarity index 100% rename from rotors_description/urdf/multirotor_base.xacro rename to models/rotors_description/urdf/multirotor_base.xacro From 608b725b7de5ef1139862ecdf2f60504487507c0 Mon Sep 17 00:00:00 2001 From: devbharat Date: Sat, 24 Oct 2015 04:03:46 +0200 Subject: [PATCH 3/5] untracked iris.sdf as we autogenerate it from xacro which are tracked --- models/iris/iris.sdf | 442 ------------------------------------------- 1 file changed, 442 deletions(-) delete mode 100644 models/iris/iris.sdf diff --git a/models/iris/iris.sdf b/models/iris/iris.sdf deleted file mode 100644 index 50ca78d277..0000000000 --- a/models/iris/iris.sdf +++ /dev/null @@ -1,442 +0,0 @@ - - - - 0 0 0 0 -0 0 - - 0 0 0 0 -0 0 - 1.5 - - 0.0347563 - 0 - 0 - 0.0458929 - 0 - 0.0977 - - - - 0 0 0 0 -0 0 - - - 0.47 0.47 0.11 - - - - - - - - - - - - - 0 0 0 0 -0 0 - - - 1 1 1 - model://rotors_description/meshes/iris.dae - - - - - - - 1 - - 0 - - - 0 0 0 0 -0 0 - - 0 0 0 0 -0 0 - 0.015 - - 1e-05 - 0 - 0 - 1e-05 - 0 - 1e-05 - - - - - iris/imu_link - base_link - - 1 0 0 - - 0 - 0 - 0 - 0 - - - 0 - 0 - - 1 - - - - 0.13 -0.22 0.023 0 -0 0 - - 0 0 0 0 -0 0 - 0.005 - - 9.75e-07 - 0 - 0 - 0.000166704 - 0 - 0.000167604 - - - - 0 0 0 0 -0 0 - - - 0.005 - 0.1 - - - - - - - - - - - - - 0 0 0 0 -0 0 - - - 1 1 1 - model://rotors_description/meshes/iris_prop_ccw.dae - - - - - - - 1 - - 0 - - - rotor_0 - base_link - - 0 0 1 - - -1e+16 - 1e+16 - - - 0 - 0 - - 1 - - - - -0.13 0.2 0.023 0 -0 0 - - 0 0 0 0 -0 0 - 0.005 - - 9.75e-07 - 0 - 0 - 0.000166704 - 0 - 0.000167604 - - - - 0 0 0 0 -0 0 - - - 0.005 - 0.1 - - - - - - - - - - - - - 0 0 0 0 -0 0 - - - 1 1 1 - model://rotors_description/meshes/iris_prop_ccw.dae - - - - - - - 1 - - 0 - - - rotor_1 - base_link - - 0 0 1 - - -1e+16 - 1e+16 - - - 0 - 0 - - 1 - - - - 0.13 0.22 0.023 0 -0 0 - - 0 0 0 0 -0 0 - 0.005 - - 9.75e-07 - 0 - 0 - 0.000166704 - 0 - 0.000167604 - - - - 0 0 0 0 -0 0 - - - 0.005 - 0.1 - - - - - - - - - - - - - 0 0 0 0 -0 0 - - - 1 1 1 - model://rotors_description/meshes/iris_prop_cw.dae - - - - - - - 1 - - 0 - - - rotor_2 - base_link - - 0 0 1 - - -1e+16 - 1e+16 - - - 0 - 0 - - 1 - - - - -0.13 -0.2 0.023 0 -0 0 - - 0 0 0 0 -0 0 - 0.005 - - 9.75e-07 - 0 - 0 - 0.000166704 - 0 - 0.000167604 - - - - 0 0 0 0 -0 0 - - - 0.005 - 0.1 - - - - - - - - - - - - - 0 0 0 0 -0 0 - - - 1 1 1 - model://rotors_description/meshes/iris_prop_cw.dae - - - - - - - 1 - - 0 - - - rotor_3 - base_link - - 0 0 1 - - -1e+16 - 1e+16 - - - 0 - 0 - - 1 - - - - iris - base_link - 10 - - - iris - rotor_0_joint - rotor_0 - ccw - 0.0125 - 0.025 - 838 - 8.54858e-06 - 0.016 - gazebo/command/motor_speed - 0 - 8.06428e-05 - 1e-06 - motor_speed/0 - 10 - - - iris - rotor_1_joint - rotor_1 - ccw - 0.0125 - 0.025 - 838 - 8.54858e-06 - 0.016 - gazebo/command/motor_speed - 1 - 8.06428e-05 - 1e-06 - motor_speed/1 - 10 - - - iris - rotor_2_joint - rotor_2 - cw - 0.0125 - 0.025 - 838 - 8.54858e-06 - 0.016 - gazebo/command/motor_speed - 2 - 8.06428e-05 - 1e-06 - motor_speed/2 - 10 - - - iris - rotor_3_joint - rotor_3 - cw - 0.0125 - 0.025 - 838 - 8.54858e-06 - 0.016 - gazebo/command/motor_speed - 3 - 8.06428e-05 - 1e-06 - motor_speed/3 - 10 - - - iris - imu - gazebo/command/motor_speed - - 0 - - iris - iris/imu_link - imu - 0.0003394 - 3.8785e-05 - 1000.0 - 0.0087 - 0.004 - 0.006 - 300.0 - 0.196 - - - From 346f06e5e917bb89290efff1111c4653c1b21914 Mon Sep 17 00:00:00 2001 From: devbharat Date: Sat, 24 Oct 2015 04:05:39 +0200 Subject: [PATCH 4/5] On second thoughts, lets keep tracking the sdf too. It does no harm. Revert "untracked iris.sdf as we autogenerate it from xacro which are tracked" This reverts commit 608b725b7de5ef1139862ecdf2f60504487507c0. --- models/iris/iris.sdf | 442 +++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 442 insertions(+) create mode 100644 models/iris/iris.sdf diff --git a/models/iris/iris.sdf b/models/iris/iris.sdf new file mode 100644 index 0000000000..50ca78d277 --- /dev/null +++ b/models/iris/iris.sdf @@ -0,0 +1,442 @@ + + + + 0 0 0 0 -0 0 + + 0 0 0 0 -0 0 + 1.5 + + 0.0347563 + 0 + 0 + 0.0458929 + 0 + 0.0977 + + + + 0 0 0 0 -0 0 + + + 0.47 0.47 0.11 + + + + + + + + + + + + + 0 0 0 0 -0 0 + + + 1 1 1 + model://rotors_description/meshes/iris.dae + + + + + + + 1 + + 0 + + + 0 0 0 0 -0 0 + + 0 0 0 0 -0 0 + 0.015 + + 1e-05 + 0 + 0 + 1e-05 + 0 + 1e-05 + + + + + iris/imu_link + base_link + + 1 0 0 + + 0 + 0 + 0 + 0 + + + 0 + 0 + + 1 + + + + 0.13 -0.22 0.023 0 -0 0 + + 0 0 0 0 -0 0 + 0.005 + + 9.75e-07 + 0 + 0 + 0.000166704 + 0 + 0.000167604 + + + + 0 0 0 0 -0 0 + + + 0.005 + 0.1 + + + + + + + + + + + + + 0 0 0 0 -0 0 + + + 1 1 1 + model://rotors_description/meshes/iris_prop_ccw.dae + + + + + + + 1 + + 0 + + + rotor_0 + base_link + + 0 0 1 + + -1e+16 + 1e+16 + + + 0 + 0 + + 1 + + + + -0.13 0.2 0.023 0 -0 0 + + 0 0 0 0 -0 0 + 0.005 + + 9.75e-07 + 0 + 0 + 0.000166704 + 0 + 0.000167604 + + + + 0 0 0 0 -0 0 + + + 0.005 + 0.1 + + + + + + + + + + + + + 0 0 0 0 -0 0 + + + 1 1 1 + model://rotors_description/meshes/iris_prop_ccw.dae + + + + + + + 1 + + 0 + + + rotor_1 + base_link + + 0 0 1 + + -1e+16 + 1e+16 + + + 0 + 0 + + 1 + + + + 0.13 0.22 0.023 0 -0 0 + + 0 0 0 0 -0 0 + 0.005 + + 9.75e-07 + 0 + 0 + 0.000166704 + 0 + 0.000167604 + + + + 0 0 0 0 -0 0 + + + 0.005 + 0.1 + + + + + + + + + + + + + 0 0 0 0 -0 0 + + + 1 1 1 + model://rotors_description/meshes/iris_prop_cw.dae + + + + + + + 1 + + 0 + + + rotor_2 + base_link + + 0 0 1 + + -1e+16 + 1e+16 + + + 0 + 0 + + 1 + + + + -0.13 -0.2 0.023 0 -0 0 + + 0 0 0 0 -0 0 + 0.005 + + 9.75e-07 + 0 + 0 + 0.000166704 + 0 + 0.000167604 + + + + 0 0 0 0 -0 0 + + + 0.005 + 0.1 + + + + + + + + + + + + + 0 0 0 0 -0 0 + + + 1 1 1 + model://rotors_description/meshes/iris_prop_cw.dae + + + + + + + 1 + + 0 + + + rotor_3 + base_link + + 0 0 1 + + -1e+16 + 1e+16 + + + 0 + 0 + + 1 + + + + iris + base_link + 10 + + + iris + rotor_0_joint + rotor_0 + ccw + 0.0125 + 0.025 + 838 + 8.54858e-06 + 0.016 + gazebo/command/motor_speed + 0 + 8.06428e-05 + 1e-06 + motor_speed/0 + 10 + + + iris + rotor_1_joint + rotor_1 + ccw + 0.0125 + 0.025 + 838 + 8.54858e-06 + 0.016 + gazebo/command/motor_speed + 1 + 8.06428e-05 + 1e-06 + motor_speed/1 + 10 + + + iris + rotor_2_joint + rotor_2 + cw + 0.0125 + 0.025 + 838 + 8.54858e-06 + 0.016 + gazebo/command/motor_speed + 2 + 8.06428e-05 + 1e-06 + motor_speed/2 + 10 + + + iris + rotor_3_joint + rotor_3 + cw + 0.0125 + 0.025 + 838 + 8.54858e-06 + 0.016 + gazebo/command/motor_speed + 3 + 8.06428e-05 + 1e-06 + motor_speed/3 + 10 + + + iris + imu + gazebo/command/motor_speed + + 0 + + iris + iris/imu_link + imu + 0.0003394 + 3.8785e-05 + 1000.0 + 0.0087 + 0.004 + 0.006 + 300.0 + 0.196 + + + From 18e8c4b15108c0d6dec7873aab13d6a313abfdbf Mon Sep 17 00:00:00 2001 From: devbharat Date: Sat, 24 Oct 2015 04:49:52 +0200 Subject: [PATCH 5/5] Modified motor constant. Ensured correct xacro files are found. Deleted the intermediate urdf file created to generate sdf. --- CMakeLists.txt | 7 +- models/iris/iris.sdf | 8 +- models/rotors_description/urdf/iris.xacro | 4 +- models/rotors_description/urdf/iris_base.urdf | 355 ------------------ .../rotors_description/urdf/iris_base.xacro | 4 +- 5 files changed, 12 insertions(+), 366 deletions(-) delete mode 100644 models/rotors_description/urdf/iris_base.urdf diff --git a/CMakeLists.txt b/CMakeLists.txt index 586960ee52..94aa37524c 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -19,15 +19,16 @@ include_directories( ${CMAKE_CURRENT_BINARY_DIR}/msgs ) -set(URDF_DIR "${CMAKE_SOURCE_DIR}/models/rotors_description/urdf") set(enable_mavlink_interface "true") set(enable_ground_truth "false") set(enable_logging "false") +set(rotors_description_dir "${CMAKE_SOURCE_DIR}/models/rotors_description") add_custom_target(sdf WORKING_DIRECTORY ${CMAKE_SOURCE_DIR} COMMAND rm -f ${CMAKE_SOURCE_DIR}/models/iris/iris.sdf - COMMAND python xacro.py -o ${URDF_DIR}/iris_base.urdf ${URDF_DIR}/iris_base.xacro enable_mavlink_interface:=${enable_mavlink_interface} enable_ground_truth:=${enable_ground_truth} enable_logging:=${enable_logging} - COMMAND gz sdf -p ${URDF_DIR}/iris_base.urdf >> ${CMAKE_SOURCE_DIR}/models/iris/iris.sdf + COMMAND python xacro.py -o ${rotors_description_dir}/urdf/iris_base.urdf ${rotors_description_dir}/urdf/iris_base.xacro enable_mavlink_interface:=${enable_mavlink_interface} enable_ground_truth:=${enable_ground_truth} enable_logging:=${enable_logging} rotors_description_dir:=${rotors_description_dir} + COMMAND gz sdf -p ${rotors_description_dir}/urdf/iris_base.urdf >> ${CMAKE_SOURCE_DIR}/models/iris/iris.sdf + COMMAND rm -f ${rotors_description_dir}/urdf/iris_base.urdf ) link_directories(${GAZEBO_LIBRARY_DIRS} ${CMAKE_CURRENT_BINARY_DIR}/msgs) diff --git a/models/iris/iris.sdf b/models/iris/iris.sdf index 50ca78d277..7f88877c82 100644 --- a/models/iris/iris.sdf +++ b/models/iris/iris.sdf @@ -360,7 +360,7 @@ 0.025 838 8.54858e-06 - 0.016 + 0.16 gazebo/command/motor_speed 0 8.06428e-05 @@ -377,7 +377,7 @@ 0.025 838 8.54858e-06 - 0.016 + 0.16 gazebo/command/motor_speed 1 8.06428e-05 @@ -394,7 +394,7 @@ 0.025 838 8.54858e-06 - 0.016 + 0.16 gazebo/command/motor_speed 2 8.06428e-05 @@ -411,7 +411,7 @@ 0.025 838 8.54858e-06 - 0.016 + 0.16 gazebo/command/motor_speed 3 8.06428e-05 diff --git a/models/rotors_description/urdf/iris.xacro b/models/rotors_description/urdf/iris.xacro index 23f88276b9..2a466be5a8 100644 --- a/models/rotors_description/urdf/iris.xacro +++ b/models/rotors_description/urdf/iris.xacro @@ -18,7 +18,7 @@ - + @@ -44,7 +44,7 @@ - + - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - iris - base_link - 10 - - - - Gazebo/DarkGrey - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - iris - rotor_0_joint - rotor_0 - ccw - 0.0125 - 0.025 - 838 - 8.54858e-06 - 0.016 - gazebo/command/motor_speed - 0 - 8.06428e-05 - 1e-06 - motor_speed/0 - 10 - - - - Gazebo/Blue - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - iris - rotor_1_joint - rotor_1 - ccw - 0.0125 - 0.025 - 838 - 8.54858e-06 - 0.016 - gazebo/command/motor_speed - 1 - 8.06428e-05 - 1e-06 - motor_speed/1 - 10 - - - - Gazebo/DarkGrey - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - iris - rotor_2_joint - rotor_2 - cw - 0.0125 - 0.025 - 838 - 8.54858e-06 - 0.016 - gazebo/command/motor_speed - 2 - 8.06428e-05 - 1e-06 - motor_speed/2 - 10 - - - - Gazebo/Blue - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - iris - rotor_3_joint - rotor_3 - cw - 0.0125 - 0.025 - 838 - 8.54858e-06 - 0.016 - gazebo/command/motor_speed - 3 - 8.06428e-05 - 1e-06 - motor_speed/3 - 10 - - - - Gazebo/DarkGrey - - - - - iris - imu - gazebo/command/motor_speed - - - - - - - - - - - - - - - - - - - - - - - iris - - iris/imu_link - - imu - - 0.0003394 - - 3.8785e-05 - - 1000.0 - - 0.0087 - - 0.004 - - 0.006 - - 300.0 - - 0.196 - - - - diff --git a/models/rotors_description/urdf/iris_base.xacro b/models/rotors_description/urdf/iris_base.xacro index 44a2d0fbea..6080a97180 100644 --- a/models/rotors_description/urdf/iris_base.xacro +++ b/models/rotors_description/urdf/iris_base.xacro @@ -1,9 +1,9 @@ - + - +