diff --git a/darwin_apps/launch/charge/charge_sim.launch b/darwin_apps/launch/charge/charge_sim.launch new file mode 100644 index 0000000000000000000000000000000000000000..8510b1f43962e3630ae44ea8f5859ffd97e0a74a --- /dev/null +++ b/darwin_apps/launch/charge/charge_sim.launch @@ -0,0 +1,19 @@ +<launch> + + <arg name="robot" default="darwin" /> + <arg name="environment" default="charge_env" /> + + <include file="$(find darwin_description)/launch/darwin_sim.launch"> + <arg name="robot" value="$(arg robot)" /> + </include> + + <!-- launch dynamic reconfigure --> + <node name="rqt_reconfigure" pkg="rqt_reconfigure" type="rqt_reconfigure" respawn="false" + output="screen"/> + + <include file="$(find darwin_description)/launch/charge_env.launch"> + <arg name="environment" value="$(arg environment)" /> + </include> + +</launch> + diff --git a/darwin_description/launch/charge_env.launch b/darwin_description/launch/charge_env.launch new file mode 100644 index 0000000000000000000000000000000000000000..7e24a80fe29339d03b6425c2cebc677ff2f39987 --- /dev/null +++ b/darwin_description/launch/charge_env.launch @@ -0,0 +1,10 @@ +<launch> + <arg name="environment" default="charge_env" /> + + <!-- Convert an xacro and put on parameter server --> + <param name="charge_environment" + command="$(find xacro)/xacro.py '$(find darwin_description)/urdf/charge/$(arg environment).xacro'" /> + + <node name="spawn_charge_station" pkg="gazebo_ros" type="spawn_model" args="-param charge_environment -urdf -model charge_station -x 1.0 -y 0.25 -z 0.03 -R 0 -P 0 -Y 3.14159"/> + +</launch> diff --git a/darwin_description/meshes/charge/charge_support.stl b/darwin_description/meshes/charge/charge_support.stl new file mode 100755 index 0000000000000000000000000000000000000000..3f0a0d183d43f5471dabca68a8fa26d3733f8167 Binary files /dev/null and b/darwin_description/meshes/charge/charge_support.stl differ diff --git a/darwin_description/meshes/charge/connector.stl b/darwin_description/meshes/charge/connector.stl new file mode 100755 index 0000000000000000000000000000000000000000..27fa7fe0987a890003b936267a52c694aec85e58 Binary files /dev/null and b/darwin_description/meshes/charge/connector.stl differ diff --git a/darwin_description/meshes/charge/far_qr.stl b/darwin_description/meshes/charge/far_qr.stl new file mode 100755 index 0000000000000000000000000000000000000000..201896091af9f860b8c06b31436aaf4ab914b3e5 Binary files /dev/null and b/darwin_description/meshes/charge/far_qr.stl differ diff --git a/darwin_description/meshes/charge/near_qr.stl b/darwin_description/meshes/charge/near_qr.stl new file mode 100755 index 0000000000000000000000000000000000000000..c229ea9b16b0d5a466ae346d8c9129ad0c291551 Binary files /dev/null and b/darwin_description/meshes/charge/near_qr.stl differ diff --git a/darwin_description/urdf/charge/charge_env.xacro b/darwin_description/urdf/charge/charge_env.xacro new file mode 100755 index 0000000000000000000000000000000000000000..c2dc91d50fd028e9442c1216fd1fd5b5d2413bc2 --- /dev/null +++ b/darwin_description/urdf/charge/charge_env.xacro @@ -0,0 +1,11 @@ +<robot name="bioloid" xmlns:xacro="http://www.ros.org/wiki/xacro"> + + <xacro:include filename="$(find bioloid_description)/urdf/ceabot/obstacle_base.xacro" /> + <xacro:include filename="$(find darwin_description)/urdf/charge/charge_station.xacro" /> + + <xacro:obstacle_base name="obstacle_base"/> + <xacro:charge_station name="station" parent="obstacle_base"> + <origin xyz="0.5 0.5 0.0" rpy="0 0 0" /> + </xacro:charge_station> + +</robot> diff --git a/darwin_description/urdf/charge/charge_station.xacro b/darwin_description/urdf/charge/charge_station.xacro new file mode 100644 index 0000000000000000000000000000000000000000..8dd1e192cc61ff5545b68759f771c56b503d0538 --- /dev/null +++ b/darwin_description/urdf/charge/charge_station.xacro @@ -0,0 +1,152 @@ +<?xml version="1.0"?> + +<root xmlns:xacro="http://ros.org/wiki/xacro"> + + <xacro:include filename="$(find bioloid_description)/urdf/qrcodes/qrcode.xacro" /> + + <xacro:macro name="charge_station" params="name parent *origin"> + <!-- charge station support --> + <link name="${name}_support_link"> + <inertial> + <mass value="0.20000000"/> + <origin xyz="0.00000000 0.00000000 0.25000000" rpy="0 0 0"/> + <inertia ixx="0.00417484" ixy="0.0" ixz="0.0" iyy="0.00417484" iyz="0.0" izz="0.00001634" /> + </inertial> + <visual> + <origin xyz="0 0 0" rpy="0 0 0"/> + <geometry> + <mesh filename="package://darwin_description/meshes/charge/charge_support.stl"/> + </geometry> + </visual> + <collision> + <origin xyz="0 0 0" rpy="0 0 0"/> + <geometry> + <mesh filename="package://darwin_description/meshes/charge/charge_support.stl"/> + </geometry> + </collision> + </link> + + <joint name="${name}_support_joint" type="fixed"> + <xacro:insert_block name="origin" /> + <parent link="${parent}_link"/> + <child link="${name}_support_link"/> + </joint> + + <!-- charge station far QR --> + <link name="${name}_far_qr_link"> + <inertial> + <mass value="0.10000000"/> + <origin xyz="0.00000000 0.00000000 0.000000" rpy="0 0 0"/> + <inertia ixx="0.00017000" ixy="0.0" ixz="0.0" iyy="0.00017000" iyz="0.0" izz="0.00017333" /> + </inertial> + <visual> + <origin xyz="0 0 0" rpy="0 0 0"/> + <geometry> + <mesh filename="package://darwin_description/meshes/charge/far_qr.stl"/> + </geometry> + </visual> + <collision> + <origin xyz="0 0 0" rpy="0 0 0"/> + <geometry> + <mesh filename="package://darwin_description/meshes/charge/far_qr.stl"/> + </geometry> + </collision> + </link> + + <joint name="${name}_far_qr_joint" type="fixed"> + <origin xyz="0.0 0.0 0.4" rpy="0 0 0"/> + <parent link="${name}_support_link"/> + <child link="${name}_far_qr_link"/> + </joint> + + <xacro:qrcode name="${name}_north" parent="${name}_far_qr" code="N1" size="0.07"> + <origin xyz="0.0501 0.0 0.0" rpy="-1.5707 0 1.5707" /> + </xacro:qrcode> + + <xacro:qrcode name="${name}_south" parent="${name}_far_qr" code="S1" size="0.07"> + <origin xyz="-0.0501 0.0 0.0" rpy="-1.5707 0 -1.5707" /> + </xacro:qrcode> + + <xacro:qrcode name="${name}_west" parent="${name}_far_qr" code="W1" size="0.07"> + <origin xyz="0.0 0.0501 0.0" rpy="1.5707 3.14159 0" /> + </xacro:qrcode> + + <xacro:qrcode name="${name}_east" parent="${name}_far_qr" code="E1" size="0.07"> + <origin xyz="0.0 -0.0501 0.0" rpy="1.5707 3.14159 3.14159 " /> + </xacro:qrcode> + + <!-- charge station connector --> + <link name="${name}_connector_link"> + <inertial> + <mass value="0.10000000"/> + <origin xyz="0.00000000 0.00000000 0.000000" rpy="0 0 0"/> + <inertia ixx="0.00014464" ixy="0.0" ixz="0.0" iyy="0.00014464" iyz="0.0" izz="0.00028761" /> + </inertial> + <visual> + <origin xyz="0 0 0" rpy="0 0 0"/> + <geometry> + <mesh filename="package://darwin_description/meshes/charge/connector.stl"/> + </geometry> + </visual> + <collision> + <origin xyz="0 0 0" rpy="0 0 0"/> + <geometry> + <mesh filename="package://darwin_description/meshes/charge/connector.stl"/> + </geometry> + </collision> + </link> + + <joint name="${name}_connector_joint" type="fixed"> + <origin xyz="0.0 0.0 0.2" rpy="0 0 0"/> + <parent link="${name}_support_link"/> + <child link="${name}_connector_link"/> + </joint> + + <!-- charge station near_qr --> + <link name="${name}_near_qr_link"> + <inertial> + <mass value="0.10000000"/> + <origin xyz="0.00000000 0.00000000 0.000000" rpy="0 0 0"/> + <inertia ixx="0.00008564" ixy="0.0" ixz="0.0" iyy="0.00008564" iyz="0.0" izz="0.00016968" /> + </inertial> + <visual> + <origin xyz="0 0 0" rpy="0 0 0"/> + <geometry> + <mesh filename="package://darwin_description/meshes/charge/near_qr.stl"/> + </geometry> + </visual> + <collision> + <origin xyz="0 0 0" rpy="0 0 0"/> + <geometry> + <mesh filename="package://darwin_description/meshes/charge/near_qr.stl"/> + </geometry> + </collision> + </link> + + <joint name="${name}_near_qr_joint" type="fixed"> + <origin xyz="0.0 0.0 0.3" rpy="0 0 0"/> + <parent link="${name}_support_link"/> + <child link="${name}_near_qr_link"/> + </joint> + + <gazebo reference="${name}_support_link"> +<!-- <gravity>true</gravity> + <selfCollide>false</selfCollide> + <maxContacts>10</maxContacts> + <dampingFactor>0.2</dampingFactor> + <maxVel>0.0</maxVel> + <minDepth>0.0</minDepth> + <mu1>0.200000</mu1> + <mu2>0.200000</mu2> + <kp>0.0</kp> + <kd>0.0</kd>--> + </gazebo> + +<!-- <gazebo reference="${name}_support_joint"> + <implicitSpringDamper>true</implicitSpringDamper> + <stopCfm>0.0</stopCfm> + <stopErp>0.0</stopErp> + </gazebo>--> + + </xacro:macro> +</root>