diff --git a/bioloid_description/urdf/ceabot/obstacle.xacro b/bioloid_description/urdf/ceabot/obstacle.xacro index 28fbd74c80efef96ab03b97c0b57f2d7b1bf8e7e..2b8677a3dfa7bb9df73e5a8ab7c40d4ed3449d16 100644 --- a/bioloid_description/urdf/ceabot/obstacle.xacro +++ b/bioloid_description/urdf/ceabot/obstacle.xacro @@ -16,7 +16,7 @@ <origin xyz="0 0 0" rpy="0 0 0"/> <geometry> <mesh filename="package://bioloid_description/meshes/ceabot/obstacle.stl"/> - </geometry> + </geometry> </visual> <collision> <origin xyz="0 0 0" rpy="0 0 0"/> diff --git a/bioloid_description/urdf/ceabot/obstacle_base.xacro b/bioloid_description/urdf/ceabot/obstacle_base.xacro index 64ddf2ed7d7100d5fd4429fee1f7741aab33c2bb..c5b0048c66f0fe90b1c05ef2f07746069b5d046b 100644 --- a/bioloid_description/urdf/ceabot/obstacle_base.xacro +++ b/bioloid_description/urdf/ceabot/obstacle_base.xacro @@ -14,6 +14,7 @@ <geometry> <mesh filename="package://bioloid_description/meshes/ceabot/obstacle_base.stl"/> </geometry> + <material name="green"/> </visual> <collision> <origin xyz="0 0 0" rpy="0 0 0"/> @@ -34,6 +35,7 @@ <geometry> <mesh filename="package://bioloid_description/meshes/ceabot/obstacle_fence.stl"/> </geometry> + <material name="white"/> </visual> <collision> <origin xyz="0 0 0" rpy="0 0 0"/> diff --git a/bioloid_description/urdf/ceabot/obstacles_env.xacro b/bioloid_description/urdf/ceabot/obstacles_env.xacro index 61f7c2fd2f566cc1e8d29844c9dab5b6b005b38a..eec111666d1200d17b03ed807399c2365d59d9f0 100755 --- a/bioloid_description/urdf/ceabot/obstacles_env.xacro +++ b/bioloid_description/urdf/ceabot/obstacles_env.xacro @@ -3,6 +3,7 @@ <xacro:include filename="$(find bioloid_description)/urdf/ceabot/obstacle_base.xacro" /> <xacro:include filename="$(find bioloid_description)/urdf/ceabot/obstacle.xacro" /> <xacro:include filename="$(find bioloid_description)/urdf/qrcodes/qrcode.xacro" /> + <xacro:include filename="$(find bioloid_description)/urdf/materials.xacro" /> <xacro:obstacle_base name="obstacle_base"/> <xacro:obstacle name="obstacle1" parent="obstacle_base" grid_x="0" grid_y="0" north_code="N1" south_code="S1" east_code="E1" west_code="W1"/> diff --git a/bioloid_description/urdf/ceabot/obstacles_env_empty.xacro b/bioloid_description/urdf/ceabot/obstacles_env_empty.xacro index 268ddb82359e323a60c8291a3f06fac4eb1bd7f3..f2a8df2bc7b9ac55d7d72604f6ceb3a2068de532 100755 --- a/bioloid_description/urdf/ceabot/obstacles_env_empty.xacro +++ b/bioloid_description/urdf/ceabot/obstacles_env_empty.xacro @@ -3,64 +3,65 @@ <xacro:include filename="$(find bioloid_description)/urdf/ceabot/obstacle_base.xacro" /> <xacro:include filename="$(find bioloid_description)/urdf/ceabot/obstacle.xacro" /> <xacro:include filename="$(find bioloid_description)/urdf/qrcodes/qrcode.xacro" /> + <xacro:include filename="$(find bioloid_description)/urdf/materials.xacro" /> <xacro:obstacle_base name="obstacle_base"/> <xacro:qrcode name="contour_west_25" parent="obstacle_base" code="W25"> - <origin xyz="0.016 0.265 0.375" rpy="0 0 -1.5707" /> + <origin xyz="0.016 0.265 0.375" rpy="0 -1.5707 0" /> </xacro:qrcode> <xacro:qrcode name="contour_west_75" parent="obstacle_base" code="W75"> - <origin xyz="0.016 0.765 0.375" rpy="0 0 -1.5707" /> + <origin xyz="0.016 0.765 0.375" rpy="0 -1.5707 0" /> </xacro:qrcode> <xacro:qrcode name="contour_west_125" parent="obstacle_base" code="W125"> - <origin xyz="0.016 1.265 0.375" rpy="0 0 -1.5707" /> + <origin xyz="0.016 1.265 0.375" rpy="0 -1.5707 0" /> </xacro:qrcode> <xacro:qrcode name="contour_west_175" parent="obstacle_base" code="W175"> - <origin xyz="0.016 1.765 0.375" rpy="0 0 -1.5707" /> + <origin xyz="0.016 1.765 0.375" rpy="0 -1.5707 0" /> </xacro:qrcode> <xacro:qrcode name="contour_west_225" parent="obstacle_base" code="W225"> - <origin xyz="0.016 2.265 0.375" rpy="0 0 -1.5707" /> + <origin xyz="0.016 2.265 0.375" rpy="0 -1.5707 0" /> </xacro:qrcode> - <xacro:qrcode name="contour_east_25" parent="obstacle_base" code="W25"> - <origin xyz="1.999 0.265 0.375" rpy="0 0 1.5707" /> + <xacro:qrcode name="contour_east_25" parent="obstacle_base" code="E25"> + <origin xyz="1.999 0.265 0.375" rpy="3.14159 -1.5707 0" /> </xacro:qrcode> - <xacro:qrcode name="contour_east_75" parent="obstacle_base" code="W75"> - <origin xyz="1.999 0.765 0.375" rpy="0 0 1.5707" /> + <xacro:qrcode name="contour_east_75" parent="obstacle_base" code="E75"> + <origin xyz="1.999 0.765 0.375" rpy="3.14159 -1.5707 0" /> </xacro:qrcode> - <xacro:qrcode name="contour_east_125" parent="obstacle_base" code="W125"> - <origin xyz="1.999 1.265 0.375" rpy="0 0 1.5707" /> + <xacro:qrcode name="contour_east_125" parent="obstacle_base" code="E125"> + <origin xyz="1.999 1.265 0.375" rpy="3.14159 -1.5707 0" /> </xacro:qrcode> - <xacro:qrcode name="contour_east_175" parent="obstacle_base" code="W175"> - <origin xyz="1.999 1.765 0.375" rpy="0 0 1.5707" /> + <xacro:qrcode name="contour_east_175" parent="obstacle_base" code="E175"> + <origin xyz="1.999 1.765 0.375" rpy="3.14159 -1.5707 0" /> </xacro:qrcode> - <xacro:qrcode name="contour_east_225" parent="obstacle_base" code="W225"> - <origin xyz="1.999 2.265 0.375" rpy="0 0 1.5707" /> + <xacro:qrcode name="contour_east_225" parent="obstacle_base" code="E225"> + <origin xyz="1.999 2.265 0.375" rpy="3.14159 -1.5707 0" /> </xacro:qrcode> <xacro:qrcode name="contour_north_25" parent="obstacle_base" code="N25"> - <origin xyz="0.265 2.514 0.375" rpy="0 0 3.14159" /> + <origin xyz="0.265 2.514 0.375" rpy="-1.5707 -1.5707 0" /> </xacro:qrcode> <xacro:qrcode name="contour_north_75" parent="obstacle_base" code="N75"> - <origin xyz="0.765 2.514 0.375" rpy="0 0 3.14159" /> + <origin xyz="0.765 2.514 0.375" rpy="-1.5707 -1.5707 0" /> </xacro:qrcode> <xacro:qrcode name="contour_north_125" parent="obstacle_base" code="N125"> - <origin xyz="1.265 2.514 0.375" rpy="0 0 3.14159" /> + <origin xyz="1.265 2.514 0.375" rpy="-1.5707 -1.5707 0" /> </xacro:qrcode> <xacro:qrcode name="contour_north_175" parent="obstacle_base" code="N175"> - <origin xyz="1.765 2.514 0.375" rpy="0 0 3.14159" /> + <origin xyz="1.765 2.514 0.375" rpy="-1.5707 -1.5707 0" /> </xacro:qrcode> <xacro:qrcode name="contour_south_25" parent="obstacle_base" code="S25"> - <origin xyz="0.265 0.016 0.375" rpy="0 0 0" /> + <origin xyz="0.265 0.016 0.375" rpy="1.5707 -1.5707 0" /> </xacro:qrcode> <xacro:qrcode name="contour_south_75" parent="obstacle_base" code="S75"> - <origin xyz="0.765 0.016 0.375" rpy="0 0 0" /> + <origin xyz="0.765 0.016 0.375" rpy="1.5707 -1.5707 0" /> </xacro:qrcode> <xacro:qrcode name="contour_south_125" parent="obstacle_base" code="S125"> - <origin xyz="1.265 0.016 0.375" rpy="0 0 0" /> + <origin xyz="1.265 0.016 0.375" rpy="1.5707 -1.5707 0" /> </xacro:qrcode> <xacro:qrcode name="contour_south_175" parent="obstacle_base" code="S175"> - <origin xyz="1.765 0.016 0.375" rpy="0 0 0" /> + <origin xyz="1.765 0.016 0.375" rpy="1.5707 -1.5707 0" /> </xacro:qrcode> </robot> diff --git a/bioloid_description/urdf/qrcodes/qrcode.xacro b/bioloid_description/urdf/qrcodes/qrcode.xacro index dd4c4f7be017ca61a5e0128ddf3bb9c558c51aeb..4ab2e29faca50b2fb2b6779a619447bb041bcded 100644 --- a/bioloid_description/urdf/qrcodes/qrcode.xacro +++ b/bioloid_description/urdf/qrcodes/qrcode.xacro @@ -4,22 +4,22 @@ <xacro:macro name="qrcode" params="name parent code *origin"> <!-- obstacle --> - <link name="${name}_${code}_link"> + <link name="${code}"> <inertial> <mass value="0.001"/> <origin xyz="0.0 0.0 0.0" rpy="0 0 0"/> <inertia ixx="0.00001" ixy="0.0" ixz="0.0" iyy="0.00001" iyz="0.0" izz="0.00001" /> </inertial> <visual> - <origin xyz="0 0 0" rpy="0 0 0"/> + <origin xyz="0 0 0" rpy="-1.5707 0 1.5707"/> <geometry> - <mesh filename="package://bioloid_description/urdf/qrcodes/${code}.dae"/> + <mesh filename="package://bioloid_description/urdf/qrcodes/${code}.dae" scale="0.725 0.725 0.725"/> </geometry> </visual> <collision> - <origin xyz="0 0 0" rpy="0 0 0"/> + <origin xyz="0 0 0" rpy="-1.5707 0 1.5707"/> <geometry> - <mesh filename="package://bioloid_description/urdf/qrcodes/${code}.dae"/> + <mesh filename="package://bioloid_description/urdf/qrcodes/${code}.dae" scale="0.725 0.725 0.725"/> </geometry> </collision> </link> @@ -27,10 +27,10 @@ <joint name="${name}_${code}_joint" type="fixed"> <xacro:insert_block name="origin" /> <parent link="${parent}_link"/> - <child link="${name}_${code}_link"/> + <child link="${code}"/> </joint> - <gazebo reference="${name}_{code}_link"> + <gazebo reference="${code}"> </gazebo> </xacro:macro> </root>