#!/usr/bin/python3.14

# Copyright (C) 2011 CNRS
#
# Author: Antonio El Khoury
#
# This program is free software: you can redistribute it and/or modify
# it under the terms of the GNU General Public License as published by
# the Free Software Foundation, either version 3 of the License, or
# (at your option) any later version.
#
# This program is distributed in the hope that it will be useful,
# but WITHOUT ANY WARRANTY; without even the implied warranty of
# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
# GNU General Public License for more details.
#
# You should have received a copy of the GNU General Public License
# along with this program.  If not, see <http://www.gnu.org/licenses/>.

import roslib; roslib.load_manifest('robot_capsule_urdf')
import rospy

import argparse

import rospkg
import csv

from urdf_parser_py.urdf import *

parser = argparse.ArgumentParser(usage='robot_capsule_urdf <options> urdf_file capsule_param\nLoad an URDF file')
parser.add_argument('urdf_file', type=argparse.FileType('r'), nargs='?',
                    default=None, help='Urdf file. Use - for stdin')
parser.add_argument('capsule_param', type=argparse.FileType('r'), nargs='?',
                    default=None, help='csv file with capsule parameters')
parser.add_argument('-o', '--output', type=argparse.FileType('w'),
                    default=None, help='Dump file to XML')
args = parser.parse_args()
# Extract robot name and directory

if args.urdf_file is None:
    robot = URDF.from_parameter_server()
else:
    robot = URDF.from_xml_string(args.urdf_file.read())

links = robot.links

param_reader = csv.reader(open(args.capsule_param.name, 'rb'),
                          delimiter=' ', quotechar='|')

for row in param_reader:
    for link in links:
        if row[0] == link.name:
            cylinder = Cylinder (float (row[2]), float (row[1]))
            link.collision.geometry = cylinder
            link.collision.origin.position[0] = float (row[3])
            link.collision.origin.position[1] = float (row[4])
            link.collision.origin.position[2] = float (row[5])
            link.collision.origin.rotation[0] = float (row[6])
            link.collision.origin.rotation[1] = float (row[7])
            link.collision.origin.rotation[2] = float (row[8])

robot.links = links
urdf = robot.to_xml_string ()
print (urdf)
