Pre course fixes (#18)

* Using variable instead of default name * Fix inertias * Fix LegacyMode again * Support initial pose for AMCL * Fix RViz config for navigation * Add slam_gmapping example * Fix RViz launchfile * Use optical link for camera * Fix position of slam_gmapping launchfile and use config YAML * Use HTTPS instead of SSH * Clean up and improve XACRO model
eborghi10 · Mar 17, 2019 · e7b8646 · e7b8646
1 parent 042f357
commit e7b8646
Show file tree

Hide file tree

Showing 23 changed files with 314 additions and 81 deletions.
diff --git a/ca_description/launch/create_description.launch b/ca_description/launch/create_description.launch
@@ -1,6 +1,6 @@
 <?xml version="1.0"?>
 <launch>
-  <arg name="model" default="$(find ca_description)/urdf/create_2.urdf.xacro"/>
+  <arg name="model" default="$(find ca_description)/urdf/create_2.xacro"/>
 
   <arg name="visualize" default="false"/>
   <arg name="rviz" default="false"/>

diff --git a/ca_description/urdf/common_properties.xacro b/ca_description/urdf/common_properties.xacro
@@ -9,4 +9,64 @@
   </inertial>
 </xacro:macro>
 
+<xacro:macro name="inertial_cuboid" params="mass x_length y_length z_length">
+    <inertial>
+      <mass value="${mass}" />
+      <origin xyz="0 0 0" />
+      <inertia ixx="${(1/12) * mass * (y_length*y_length + z_length*z_length)}" ixy="0.0" ixz="0.0"
+               iyy="${(1/12) * mass * (x_length*x_length + z_length*z_length)}" iyz="0.0"
+               izz="${(1/12) * mass * (x_length*x_length + y_length*y_length)}" />
+    </inertial>
+  </xacro:macro>
+
+  <xacro:macro name="inertial_cuboid_with_pose" params="mass x_length y_length z_length *origin">
+    <inertial>
+      <xacro:insert_block name="origin"/>
+      <mass value="${mass}" />
+      <inertia ixx="${(1/12) * mass * (y_length*y_length + z_length*z_length)}" ixy="0.0" ixz="0.0"
+               iyy="${(1/12) * mass * (x_length*x_length + z_length*z_length)}" iyz="0.0"
+               izz="${(1/12) * mass * (x_length*x_length + y_length*y_length)}" />
+    </inertial>
+  </xacro:macro>
+
+  <xacro:macro name="inertial_cylinder" params="mass radius length">
+    <inertial>
+      <mass value="${mass}"/>
+      <origin xyz="0 0 0" />
+      <inertia ixx="${(1/12) * mass * (3*radius*radius + length*length)}" ixy="0.0" ixz="0.0"
+               iyy="${(1/12) * mass * (3*radius*radius + length*length)}" iyz="0.0"
+               izz="${(1/2) * mass * (radius*radius)}" />
+    </inertial>
+  </xacro:macro>
+
+  <xacro:macro name="inertial_cylinder_with_pose" params="mass radius length *origin">
+    <inertial>
+      <xacro:insert_block name="origin"/>
+      <mass value="${mass}" />
+      <inertia ixx="${(1/12) * mass * (3*radius*radius + length*length)}" ixy="0.0" ixz="0.0"
+	             iyy="${(1/12) * mass * (3*radius*radius + length*length)}" iyz="0.0"
+	             izz="${(1/2) * mass * (radius*radius)}" />
+    </inertial>
+  </xacro:macro>
+
+  <xacro:macro name="inertial_sphere" params="mass diameter">
+    <inertial>
+      <mass value="${mass}" />
+      <origin xyz="0 0 0" />
+      <inertia ixx="${(2/5) * mass * ( (diameter*0.5) * (diameter*0.5) )}" ixy="0.0" ixz="0.0"
+	             iyy="${(2/5) * mass * ( (diameter*0.5) * (diameter*0.5) )}" iyz="0.0"
+	             izz="${(2/5) * mass * ( (diameter*0.5) * (diameter*0.5) )}" />
+    </inertial>
+  </xacro:macro>
+
+  <xacro:macro name="inertial_sphere_with_pose" params="mass diameter *origin">
+    <inertial>
+      <xacro:insert_block name="origin"/>
+      <mass value="${mass}" />
+      <inertia ixx="${(2/5) * mass * ( (diameter*0.5) * (diameter*0.5) )}" ixy="0.0" ixz="0.0"
+	             iyy="${(2/5) * mass * ( (diameter*0.5) * (diameter*0.5) )}" iyz="0.0"
+	             izz="${(2/5) * mass * ( (diameter*0.5) * (diameter*0.5) )}" />
+    </inertial>
+  </xacro:macro>
+
 </robot>
diff --git a/ca_description/urdf/create_2.urdf.xacro b/ca_description/urdf/create_2.urdf.xacro
diff --git a/ca_description/urdf/create_2.xacro b/ca_description/urdf/create_2.xacro
@@ -0,0 +1,21 @@
+<?xml version="1.0" ?>
+<robot name="create_2" xmlns:xacro="http://ros.org/wiki/xacro">
+  <xacro:include filename="$(find ca_description)/urdf/create_base.xacro"/>
+  <xacro:include filename="$(find ca_description)/urdf/roomblock.xacro"/>
+
+  <xacro:property name="wheel_separation" value="0.235"/>
+  <xacro:property name="base_diameter" value="0.3485"/>
+  <xacro:property name="wheel_width" value="0.024"/>
+  <xacro:property name="wheel_radius" value="0.036"/>
+
+  <xacro:create_base wheel_separation="${wheel_separation}" 
+                     base_diameter="${base_diameter}" 
+                     wheel_width="${wheel_width}"
+                     wheel_radius="${wheel_radius}"
+                     visualize="$(arg visualize)">
+    <mesh filename="package://ca_description/meshes/create_2.dae" />
+  </xacro:create_base>
+
+  <!-- Roomblock -->
+  <xacro:roomblock parent="base_link" visualize="$(arg visualize)"/>
+</robot>
diff --git a/ca_description/urdf/create_base.urdf.xacro → ca_description/urdf/create_base.xacro b/ca_description/urdf/create_base.urdf.xacro → ca_description/urdf/create_base.xacro
@@ -1,60 +1,46 @@
 <?xml version="1.0" ?>
 <robot xmlns:xacro="http://ros.org/wiki/xacro">
 
-  <xacro:macro name="create_base" params="diffdrive_update_rate:=40 wheel_separation wheel_radius:=0.036 wheel_width:=0.024 wheel_torque:=0.5 wheel_accel:=0.5 mass_kg:=3.5 base_diameter *mesh visualize:=false">
+  <xacro:macro name="create_base" params="wheel_separation wheel_radius wheel_width base_diameter *mesh visualize:=false">
 
-    <xacro:include filename="$(find ca_description)/urdf/create_base_gazebo.urdf.xacro"/>
+    <xacro:include filename="$(find ca_description)/urdf/create_base_gazebo.xacro"/>
     <xacro:include filename="$(find ca_description)/urdf/create_caster_wheel.xacro"/>
     <xacro:include filename="$(find ca_description)/urdf/create_wheel.xacro"/>
     <xacro:include filename="$(find ca_description)/urdf/sensors/imu_sensor.xacro"/>
     <xacro:include filename="$(find ca_description)/urdf/sensors/wall_sensor.xacro"/>
     <xacro:include filename="$(find ca_description)/urdf/sensors/cliff_sensors.xacro"/>
 
-    <link name="base_footprint">
-    <visual>
-      <origin xyz="0 0 0" rpy="0 0 0" />
-      <geometry>
-        <box size="0.001 0.001 0.001" />
-      </geometry>
-      <material name="Green" />
-    </visual>
+    <xacro:property name="base_link_offset_z" value="0.0308"/>
+    <xacro:property name="base_offset_z" value="0.017"/>
+    <xacro:property name="base_height" value="0.0611632"/>
+    <xacro:property name="base_mass" value="2"/>
 
-    <collision>
-      <origin xyz="0 0 0.017" rpy="0 0 0" />
-      <geometry>
-        <box size="0.001 0.001 0.001" />
-      </geometry>
-    </collision>
-    </link>
+    <link name="base_footprint"/>
+
+    <joint name="base_footprint_joint" type="fixed">
+      <origin xyz="0 0 ${base_offset_z}" rpy="0 0 0" />
+      <parent link="base_footprint" />
+      <child link="base_link" />
+    </joint>
 
     <link name="base_link">
-      <inertial>
-        <mass value="2" />
-        <origin xyz="0 0 0.0" />
-        <inertia ixx="0.01" ixy="0.0" ixz="0.0" iyy="0.01" iyz="0.0" izz="0.5" />
-      </inertial>
+      <xacro:inertial_cylinder mass="${base_mass}" radius="${base_diameter/2}" length="${base_height}"/>
 
       <visual>
-        <origin xyz=" 0 0 0.0308" rpy="0 0 0" />
+        <origin xyz=" 0 0 ${base_link_offset_z}" rpy="0 0 0" />
         <geometry>
           <xacro:insert_block name="mesh" />
         </geometry>
       </visual>
 
       <collision>
-        <origin xyz="0.0 0.0 0.0308" rpy="0 0 0" />
+        <origin xyz="0 0 ${base_link_offset_z}" rpy="0 0 0" />
         <geometry>
-          <cylinder length="0.0611632" radius="0.16495" />
+          <cylinder length="${base_height}" radius="${base_diameter/2}" />
         </geometry>
       </collision>
     </link>
 
-    <joint name="base_footprint_joint" type="fixed">
-      <origin xyz="0 0 0.017" rpy="0 0 0" />
-      <parent link="base_footprint" />
-      <child link="base_link" />
-    </joint>
-
     <xacro:wall_sensor visualize="${visualize}"/>
     <xacro:cliff_sensors visualize="${visualize}"/>
     <xacro:imu_sensor/>

diff --git a/...iption/urdf/create_base_gazebo.urdf.xacro → ca_description/urdf/create_base_gazebo.xacro b/...iption/urdf/create_base_gazebo.urdf.xacro → ca_description/urdf/create_base_gazebo.xacro
@@ -1,14 +1,19 @@
 <?xml version="1.0" ?>
 <robot xmlns:xacro="http://ros.org/wiki/xacro">
   <xacro:macro name="sim_create_base">
+
+    <xacro:property name="diffdrive_update_rate" value="40"/>
+    <xacro:property name="wheel_torque" value="0.5"/>
+    <xacro:property name="wheel_accel" value="0.5"/>
+
     <gazebo>
       <!-- Diff-drive controller -->
       <plugin name="differential_drive_controller" filename="libgazebo_ros_diff_drive.so">
         <alwaysOn>true</alwaysOn>
         <rosDebugLevel>na</rosDebugLevel>
         <updateRate>${diffdrive_update_rate}</updateRate>
-        <leftJoint>right_wheel_joint</leftJoint>
-        <rightJoint>left_wheel_joint</rightJoint>
+        <leftJoint>left_wheel_joint</leftJoint>
+        <rightJoint>right_wheel_joint</rightJoint>
         <wheelSeparation>${wheel_separation}</wheelSeparation>
         <wheelDiameter>${wheel_radius * 2}</wheelDiameter>
         <wheelTorque>${wheel_torque}</wheelTorque>
@@ -28,7 +33,7 @@
       <!-- Ground truth -->
       <plugin name="p3d_base_controller" filename="libgazebo_ros_p3d.so">
         <alwaysOn>true</alwaysOn>
-        <updateRate>100.0</updateRate>
+        <updateRate>60.0</updateRate>
         <bodyName>base_link</bodyName>
         <topicName>gts</topicName>
         <gaussianNoise>0</gaussianNoise>

diff --git a/ca_description/urdf/create_caster_wheel.xacro b/ca_description/urdf/create_caster_wheel.xacro
@@ -18,7 +18,7 @@
   <xacro:property name="radius" value="0.018"/>
 
   <link name="${link_name}">
-    <xacro:dummy_inertia/>
+    <xacro:inertial_sphere mass="0.05" diameter="${2*radius}"/>
 
     <visual>
       <origin xyz="0 0 0" rpy="${R} ${P} ${Y}" />

diff --git a/ca_description/urdf/create_wheel.xacro b/ca_description/urdf/create_wheel.xacro
@@ -13,7 +13,9 @@
   <xacro:property name="Y" value="${pi/2}"/>
 
   <link name="${prefix}_wheel_link">
-    <xacro:dummy_inertia/>
+    <xacro:inertial_cylinder_with_pose mass="0.1" radius="${wheel_radius}" length="${wheel_width}">
+      <origin xyz="0 0 0" rpy="${R} ${P} ${Y}" />
+    </xacro:inertial_cylinder_with_pose>
 
     <visual>
       <origin xyz="0 0 0" rpy="${R} ${P} ${Y}" />
@@ -34,7 +36,7 @@
     <origin xyz="${x} ${y} ${z}" rpy="0 0 0" />
     <parent link="base_link" />
     <child link="${prefix}_wheel_link" />
-    <axis xyz="0 -1 0" />
+    <axis xyz="0 1 0" />
   </joint>
 </xacro:macro>
 

diff --git a/ca_description/urdf/roomblock.urdf.xacro → ca_description/urdf/roomblock.xacro b/ca_description/urdf/roomblock.urdf.xacro → ca_description/urdf/roomblock.xacro
@@ -13,23 +13,33 @@ Block on top of the Create 2 that contains a Raspicam and an RPLidar
     <xacro:property name="z" value="0.061"/>
     <xacro:property name="roomblock_link" value="block_link"/>
     <xacro:property name="parent_link" value="${parent}"/>
+    <xacro:property name="roomblock_mass" value="0.1"/>
+    <xacro:property name="roomblock_size_x" value="0.1"/>
+    <xacro:property name="roomblock_size_y" value="0.1"/>
+    <xacro:property name="roomblock_size_z" value="0.07"/>
 
     <joint name="block_joint" type="fixed">
       <origin xyz="0 0 ${z}" rpy="0 0 0" />
       <parent link="${parent_link}"/>
       <child link="${roomblock_link}" />
     </joint>
+
     <link name="${roomblock_link}">
       <visual>
         <geometry>
   	       <mesh filename="package://ca_description/meshes/block.dae"/>
         </geometry>
         <material name="Grey" />
       </visual>
+      <xacro:inertial_cuboid_with_pose mass="${roomblock_mass}" 
+                                       x_length="${roomblock_size_x}" 
+                                       y_length="${roomblock_size_y}" 
+                                       z_length="${roomblock_size_z}">
+        <origin xyz="0 0 ${roomblock_size_z/2}" rpy="0 0 0"/>
+      </xacro:inertial_cuboid_with_pose>
     </link>
 
     <xacro:laser_sensor parent="${roomblock_link}" visualize="$(arg visualize)"/>
-
     <xacro:camera_sensor parent="${roomblock_link}"/>
 
     <xacro:roomblock_gazebo link_name="${roomblock_link}"/>

diff --git a/ca_description/urdf/roomblock_gazebo.xacro b/ca_description/urdf/roomblock_gazebo.xacro
@@ -2,7 +2,7 @@
 <robot xmlns:xacro="http://ros.org/wiki/xacro">
   <xacro:macro name="roomblock_gazebo" params="link_name">
 
-    <gazebo reference="block_link">
+    <gazebo reference="${link_name}">
       <material>Gazebo/Grey</material>
     </gazebo>
 

diff --git a/ca_description/urdf/sensors/camera_sensor.xacro b/ca_description/urdf/sensors/camera_sensor.xacro
@@ -6,23 +6,32 @@
 
   <xacro:property name="parent_link" value="${parent}"/>
   <xacro:property name="camera_link" value="raspicam_link"/>
+  <xacro:property name="optical_link" value="raspicam_link_optical"/>
 
   <xacro:property name="x" value="0.04"/>
   <xacro:property name="y" value="0.0"/>
   <xacro:property name="z" value="0.1"/>
-  <xacro:property name="R" value="0.0"/>
+  <xacro:property name="R" value="${-pi/2}"/>
   <xacro:property name="P" value="0.0"/>
-  <xacro:property name="Y" value="0.0"/>
+  <xacro:property name="Y" value="${-pi/2}"/>
 
   <joint name="camera_joint" type="fixed">
-    <origin xyz="${x} ${y} ${z}" rpy="${R} ${P} ${Y}" />
+    <origin xyz="${x} ${y} ${z}" rpy="0 0 0" />
     <parent link="${parent_link}" />
     <child link="${camera_link}" />
   </joint>
   <link name="${camera_link}"/>
 
+  <joint name="optical_joint" type="fixed">
+    <origin xyz="0 0 0" rpy="${R} ${P} ${Y}" />
+    <parent link="${camera_link}" />
+    <child link="${optical_link}" />
+  </joint>
+  <link name="${optical_link}"/>
+
+
   <!-- Gazebo plugin -->
-  <xacro:camera_sensor_gazebo link_name="${camera_link}"/>
+  <xacro:camera_sensor_gazebo link_name="${optical_link}"/>
 
 </xacro:macro>
 </robot>
diff --git a/ca_description/urdf/sensors/camera_sensor_gazebo.xacro b/ca_description/urdf/sensors/camera_sensor_gazebo.xacro
@@ -5,7 +5,7 @@
 
   <gazebo reference="${link_name}">
     <material>Gazebo/Orange</material>
-    <sensor type="camera" name="camera1">
+    <sensor type="camera" name="raspicam">
       <update_rate>30.0</update_rate>
       <camera name="head">
         <horizontal_fov>1.3962634</horizontal_fov>
@@ -34,7 +34,7 @@
         <imageTopicName>image_raw</imageTopicName>
         <cameraInfoTopicName>camera_info</cameraInfoTopicName>
         <frameName>${link_name}</frameName>
-        <hackBaseline>0.07</hackBaseline>
+        <hackBaseline>0.0</hackBaseline>
         <distortionK1>0.0</distortionK1>
         <distortionK2>0.0</distortionK2>
         <distortionK3>0.0</distortionK3>

diff --git a/ca_tools/launch/rviz.launch b/ca_tools/launch/rviz.launch
@@ -1,5 +1,5 @@
 <?xml version="1.0"?>
 <launch>
-  <arg name="config_file" default=""/>
+  <arg name="config_file" default="default"/>
   <node type="rviz" name="rviz" pkg="rviz" args="-d $(arg config_file)" required="false"/>
 </launch>
diff --git a/navigation/ca_move_base/config/amcl_params.yaml b/navigation/ca_move_base/config/amcl_params.yaml
@@ -1,7 +1,4 @@
 use_map_topic: false
-initial_pose_x: 0.0
-initial_pose_y: 0.0
-initial_pose_a: 0.0
 odom_frame_id: "odom"
 base_frame_id: "base_link"
 global_frame_id: "map"

diff --git a/navigation/ca_move_base/launch/amcl.launch b/navigation/ca_move_base/launch/amcl.launch
@@ -1,5 +1,11 @@
 <launch>
+  <arg name="initial_pose_x"  default="0.0"/>
+  <arg name="initial_pose_y"  default="0.0"/>
+  <arg name="initial_pose_a"  default="0.0"/>
   <node pkg="amcl" type="amcl" name="amcl">
+    <param name="initial_pose_x" value="$(arg initial_pose_x)"/>
+    <param name="initial_pose_y" value="$(arg initial_pose_y)"/>
+    <param name="initial_pose_a" value="$(arg initial_pose_a)"/>
     <rosparam file="$(find ca_move_base)/config/amcl_params.yaml" command="load"/>
     <remap from="scan" to="laser/scan"/>
   </node>

diff --git a/navigation/ca_move_base/launch/navigate.launch b/navigation/ca_move_base/launch/navigate.launch
@@ -12,7 +12,14 @@
 	<include file="$(find ca_move_base)/launch/move_base.launch" />
 
 	<!-- AMCL -->
-  <include file="$(find ca_move_base)/launch/amcl.launch"/>
+  <arg name="initial_pose_x"  default="0.0"/>
+  <arg name="initial_pose_y"  default="0.0"/>
+  <arg name="initial_pose_a"  default="0.0"/>
+  <include file="$(find ca_move_base)/launch/amcl.launch">
+    <arg name="initial_pose_x" value="$(arg initial_pose_x)"/>
+    <arg name="initial_pose_y" value="$(arg initial_pose_y)"/>
+    <arg name="initial_pose_a" value="$(arg initial_pose_a)"/>
+  </include>
 
 	<!-- RViz -->
 	<include if="$(arg rviz)" file="$(find ca_tools)/launch/rviz.launch">