<?xml version="1.0"?>

<robot name="drawer" xmlns:xacro="http://www.ros.org/wiki/xacro">

	<material name="light_turquoise">
		<color rgba="0 0.6 0.6 1"/>
	</material>
	<material name="light_purple">
		<color rgba="0.3 0.3 0.6 1"/>
	</material>

	<xacro:property name="pi" value="3.14159265359"/>

	<xacro:macro name="drawer" params="prefix planks_thickness drawer_sizeX drawer_sizeY drawer_sizeZ drawer_handle_radius">

		<link name="${prefix}_drawer_bottom">
			<visual>
				<geometry>
					<box size="${drawer_sizeX - (2*planks_thickness)} ${drawer_sizeY - planks_thickness} ${planks_thickness}"/>
				</geometry>
				<origin xyz="0 0 0" rpy="0 0 0"/>
				<material name="light_turquoise"/>
			</visual>
			<collision>
				<geometry>
					<box size="${drawer_sizeX - (2*planks_thickness)} ${drawer_sizeY - planks_thickness} ${planks_thickness}"/>
				</geometry>
				<origin xyz="0 0 0" rpy="0 0 0"/>
			</collision>
		</link>

		<link name="${prefix}_drawer_background">
			<visual>
				<geometry>
					<box size="${drawer_sizeX - (2*planks_thickness)} ${planks_thickness} ${drawer_sizeZ - (2*planks_thickness)}"/>
				</geometry>
				<origin xyz="0 0 0" rpy="0 0 0"/>
				<material name="light_turquoise"/>
			</visual>
			<collision>
				<geometry>
					<box size="${drawer_sizeX - (2*planks_thickness)} ${planks_thickness} ${drawer_sizeZ - (2*planks_thickness)}"/>
				</geometry>
				<origin xyz="0 0 0" rpy="0 0 0"/>
			</collision>
		</link>

		<joint name="${prefix}_drawer_bottom_background_joint" type="fixed">
			<parent link="${prefix}_drawer_bottom"/>
			<child link="${prefix}_drawer_background"/>
			<origin xyz="0 ${((drawer_sizeY - planks_thickness)/2) - (planks_thickness/2)} ${((drawer_sizeZ - (2*planks_thickness))/2) + (planks_thickness/2)}" rpy="0 0 0"/>
		</joint>

		<link name="${prefix}_drawer_right">
			<visual>
				<geometry>
					<box size="${planks_thickness} ${drawer_sizeY - (2*planks_thickness)} ${drawer_sizeZ - (2*planks_thickness)}"/>
				</geometry>
				<origin xyz="0 0 0" rpy="0 0 0"/>
				<material name="light_turquoise"/>
			</visual>
			<collision>
				<geometry>
					<box size="${planks_thickness} ${drawer_sizeY - (2*planks_thickness)} ${drawer_sizeZ - (2*planks_thickness)}"/>
				</geometry>
				<origin xyz="0 0 0" rpy="0 0 0"/>
			</collision>
		</link>

		<joint name="${prefix}_drawer_bottom_right_joint" type="fixed">
			<parent link="${prefix}_drawer_bottom"/>
			<child link="${prefix}_drawer_right"/>
			<origin xyz="${((drawer_sizeX - (2*planks_thickness))/2) - (planks_thickness/2)} ${-planks_thickness/2} ${((drawer_sizeZ - (2*planks_thickness))/2) + (planks_thickness/2)}" rpy="0 0 0"/>
		</joint>

		<link name="${prefix}_drawer_left">
			<visual>
				<geometry>
					<box size="${planks_thickness} ${drawer_sizeY - (2*planks_thickness)} ${drawer_sizeZ - (2*planks_thickness)}"/>
				</geometry>
				<origin xyz="0 0 0" rpy="0 0 0"/>
				<material name="light_turquoise"/>
			</visual>
			<collision>
				<geometry>
					<box size="${planks_thickness} ${drawer_sizeY - (2*planks_thickness)} ${drawer_sizeZ - (2*planks_thickness)}"/>
				</geometry>
				<origin xyz="0 0 0" rpy="0 0 0"/>
			</collision>
		</link>

		<joint name="${prefix}_drawer_bottom_left_joint" type="fixed">
			<parent link="${prefix}_drawer_bottom"/>
			<child link="${prefix}_drawer_left"/>
			<origin xyz="${-((drawer_sizeX - (2*planks_thickness))/2) + (planks_thickness/2)} ${-planks_thickness/2} ${((drawer_sizeZ - (2*planks_thickness))/2) + (planks_thickness/2)}" rpy="0 0 0"/>
		</joint>

		<link name="${prefix}_drawer_front">
			<visual>
				<geometry>
					<box size="${drawer_sizeX} ${planks_thickness} ${drawer_sizeZ}"/>
				</geometry>
				<origin xyz="0 0 0" rpy="0 0 0"/>
				<material name="light_turquoise"/>
			</visual>
			<collision>
				<geometry>
					<box size="${drawer_sizeX} ${planks_thickness} ${drawer_sizeZ}"/>
				</geometry>
				<origin xyz="0 0 0" rpy="0 0 0"/>
			</collision>
		</link>

		<joint name="${prefix}_drawer_bottom_front_joint" type="fixed">
			<parent link="${prefix}_drawer_bottom"/>
			<child link="${prefix}_drawer_front"/>
			<origin xyz="0 ${-((drawer_sizeY - planks_thickness)/2) - (planks_thickness/2)} ${(drawer_sizeZ/2) - (3*planks_thickness/2)}" rpy="0 0 0"/>
		</joint>

		<link name="${prefix}_drawer_handle">
			<visual>
				<geometry>
					<sphere radius="${drawer_handle_radius}"/>
				</geometry>
				<origin xyz="0 0 0" rpy="0 0 ${pi/2}"/>
				<material name="light_purple"/>
			</visual>
			<collision>
				<geometry>
					<sphere radius="${drawer_handle_radius}"/>
				</geometry>
				<origin xyz="0 0 0" rpy="0 0 ${pi/2}"/>
			</collision>
		</link>

		<joint name="${prefix}_drawer_front_handle_joint" type="fixed">
			<parent link="${prefix}_drawer_front"/>
			<child link="${prefix}_drawer_handle"/>
			<origin xyz="0 ${-drawer_handle_radius - (planks_thickness/2)} 0" rpy="0 0 0"/>
		</joint>

	</xacro:macro>

</robot>
