mobility_base_tools: urdf_remove_pedestal.py Source File

Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 
00003 # Software License Agreement (BSD License)
00004 #
00005 # Copyright (c) 2014-2017, Dataspeed Inc.
00006 # All rights reserved.
00007 #
00008 # Redistribution and use in source and binary forms, with or without modification,
00009 # are permitted provided that the following conditions are met:
00010 # 
00011 #     * Redistributions of source code must retain the above copyright notice,
00012 #       this list of conditions and the following disclaimer.
00013 #     * Redistributions in binary form must reproduce the above copyright notice,
00014 #       this list of conditions and the following disclaimer in the documentation
00015 #       and/or other materials provided with the distribution.
00016 #     * Neither the name of Dataspeed Inc. nor the names of its
00017 #       contributors may be used to endorse or promote products derived from this
00018 #       software without specific prior written permission.
00019 #
00020 # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
00021 # ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
00022 # WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
00023 # DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
00024 # FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
00025 # DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
00026 # SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00027 # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
00028 # OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
00029 # OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
00030 
00031 import rospy
00032 import re
00033 
00034 match_link = "(.*)<link[^>]*name\s*=\s*\"pedestal\"[^>]*>.*?[^<]<\/link>(.*)"
00035 match_joint = "(.*)<joint[^>]*name\s*=\s*\"pedestal_fixed\"[^>]*>.*?[^<]<\/joint>(.*)"
00036 
00037 if __name__ == '__main__':
00038     try:
00039         rospy.init_node('urdf_remove_pedestal', anonymous=True)
00040         param_src = rospy.get_param('~param_src', "/robot_description")
00041         param_dest = rospy.get_param('~param_dest', "/robot_description_mod")
00042         urdf = rospy.get_param(param_src, "")
00043         changed = False
00044         if urdf:
00045             obj = re.match(match_link, urdf, re.S)
00046             if obj:
00047                 urdf = obj.group(1) + obj.group(2)
00048                 changed = True
00049                 rospy.loginfo("Removed link 'pedestal'")
00050             else:
00051                 rospy.logwarn("Failed to find link 'pedestal'")
00052             
00053             obj = re.match(match_joint, urdf, re.S)
00054             if obj:
00055                 urdf = obj.group(1) + obj.group(2)
00056                 changed = True
00057                 rospy.loginfo("Removed joint 'pedestal_fixed'")
00058             else:
00059                 rospy.logwarn("Failed to find joint 'pedestal_fixed'")
00060             
00061             rospy.set_param(param_dest, urdf)
00062             if changed:
00063                 rospy.loginfo("Updated parameter '%s'", param_dest)
00064             else:
00065                 rospy.loginfo("Copied parameter '%s' to '%s'", param_src, param_dest)
00066         else:
00067             rospy.logwarn("Parameter '%s' not found", param_src)
00068     except rospy.ROSInterruptException: pass
00069 
00070