Unverified Commit 09d001a6 authored by Simon Thomas's avatar Simon Thomas
Browse files

First version for ukf with all sensors

parent eb869bad
odom_frame: odom odom_frame: odom
base_link_frame: base_link base_link_frame: base_link
world_frame: odom world_frame: map
two_d_mode: true two_d_mode: true
...@@ -15,6 +15,24 @@ odom0_config: [false, false, false, ...@@ -15,6 +15,24 @@ odom0_config: [false, false, false,
odom0_differential: false odom0_differential: false
odom0_queue_size: 10 odom0_queue_size: 10
odom1: odometry/gps
odom1_config: [true, true, true,
false, false, false,
false, false, false,
false, false, false,
false, false, false]
odom1_differential: false
odom1_queue_size: 10
odom2: rtabmap/odom
odom2_config: [true, true, true,
true, true, true,
false, false, false,
false, false, false,
false, false, false]
odom2_differential: true
odom2_queue_size: 10
imu0: imu/data imu0: imu/data
imu0_config: [false, false, false, imu0_config: [false, false, false,
true, true, true, true, true, true,
......
odom_frame: odom
base_link_frame: base_link
world_frame: odom
two_d_mode: true
frequency: 50
odom0: husky_velocity_controller/odom
odom0_config: [false, false, false,
false, false, false,
true, true, true,
false, false, true,
false, false, false]
odom0_differential: false
odom0_queue_size: 10
imu0: imu/data
imu0_config: [false, false, false,
true, true, true,
false, false, false,
true, true, true,
false, false, false]
imu0_differential: true
imu0_queue_size: 10
imu0_remove_gravitational_acceleration: true
...@@ -5,12 +5,11 @@ ...@@ -5,12 +5,11 @@
<arg name="simulation" default="$(optenv ROBOT_SIMULATION false)"/> <arg name="simulation" default="$(optenv ROBOT_SIMULATION false)"/>
<arg name="robot_namespace" default="$(optenv ROBOT_NAMESPACE robot)"/> <arg name="robot_namespace" default="$(optenv ROBOT_NAMESPACE robot)"/>
<arg name="config_extras" # <arg name="config_extras"
default="$(eval optenv('HUSKY_CONFIG_EXTRAS', find('husky_control') + '/config/empty.yaml'))"/> default="$(eval optenv('HUSKY_CONFIG_EXTRAS', find('husky_control') + '/config/empty.yaml'))"/>
<arg name="laser_enabled" default="$(optenv HUSKY_LMS1XX_ENABLED false)"/> <arg name="laser_enabled" default="$(optenv HUSKY_LMS1XX_ENABLED false)"/>
<arg name="kinect_enabled" default="$(optenv HUSKY_KINECT_ENABLED false)"/> <arg name="kinect_enabled" default="$(optenv HUSKY_KINECT_ENABLED false)"/>
<arg name="urdf_extras" default="$(optenv HUSKY_URDF_EXTRAS)"/>
<include file="$(find multimaster_launch)/launch/multimaster_robot.launch" if="$(arg multimaster)"> <include file="$(find multimaster_launch)/launch/multimaster_robot.launch" if="$(arg multimaster)">
<arg name="gazebo_interface" value="$(find husky_control)/config/gazebo_interface.yaml" /> <arg name="gazebo_interface" value="$(find husky_control)/config/gazebo_interface.yaml" />
...@@ -23,7 +22,6 @@ ...@@ -23,7 +22,6 @@
<include file="$(find husky_description)/launch/description.launch" > <include file="$(find husky_description)/launch/description.launch" >
<arg name="laser_enabled" default="$(arg laser_enabled)"/> <arg name="laser_enabled" default="$(arg laser_enabled)"/>
<arg name="kinect_enabled" default="$(arg kinect_enabled)"/> <arg name="kinect_enabled" default="$(arg kinect_enabled)"/>
<arg name="urdf_extras" default="$(arg urdf_extras)"/>
</include> </include>
<!-- Load controller configuration --> <!-- Load controller configuration -->
...@@ -33,14 +31,14 @@ ...@@ -33,14 +31,14 @@
<node name="base_controller_spawner" pkg="controller_manager" type="spawner" <node name="base_controller_spawner" pkg="controller_manager" type="spawner"
args="husky_joint_publisher husky_velocity_controller"/> args="husky_joint_publisher husky_velocity_controller"/>
<!-- Start EKF for localization --> <!-- Start UKF for localization -->
<node pkg="robot_localization" type="ekf_localization_node" name="ekf_localization"> <node pkg="robot_localization" type="ukf_localization_node" name="ukf_localization">
<rosparam command="load" file="$(find husky_control)/config/localization.yaml" /> <rosparam command="load" file="$(find husky_control)/config/localization.yaml" />
</node> </node>
<node pkg="interactive_marker_twist_server" type="marker_server" name="twist_marker_server" output="screen"/> <node pkg="interactive_marker_twist_server" type="marker_server" name="twist_marker_server" output="screen"/>
<node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" /> <node name="robot_state_publisher" pkg="robot_state_publisher" type="state_publisher" />
<node pkg="twist_mux" type="twist_mux" name="twist_mux"> <node pkg="twist_mux" type="twist_mux" name="twist_mux">
<rosparam command="load" file="$(find husky_control)/config/twist_mux.yaml" /> <rosparam command="load" file="$(find husky_control)/config/twist_mux.yaml" />
...@@ -48,6 +46,5 @@ ...@@ -48,6 +46,5 @@
</node> </node>
<!-- Override the default control parameters, see config/empty.yaml for default. --> <!-- Override the default control parameters, see config/empty.yaml for default. -->
<rosparam command="load" file="$(arg config_extras)" />
</launch> </launch>
...@@ -25,17 +25,14 @@ ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSI ...@@ -25,17 +25,14 @@ ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSI
--> -->
<launch> <launch>
<arg name="robot_namespace" default="/"/> <arg name="robot_namespace" default="husky"/>
<arg name="laser_enabled" default="$(optenv HUSKY_LMS1XX_ENABLED false)"/> <arg name="laser_enabled" default="$(optenv HUSKY_LMS1XX_ENABLED false)"/>
<arg name="kinect_enabled" default="$(optenv HUSKY_KINECT_ENABLED false)"/> <arg name="kinect_enabled" default="$(optenv HUSKY_KINECT_ENABLED false)"/>
<arg name="urdf_extras" default="$(optenv HUSKY_URDF_EXTRAS)"/>
<param name="robot_description" command="$(find xacro)/xacro '$(find husky_description)/urdf/husky.urdf.xacro' <param name="robot_description" command="$(find xacro)/xacro '$(find erl_simulations)/models/husky.urdf.xacro'
--inorder
robot_namespace:=$(arg robot_namespace) robot_namespace:=$(arg robot_namespace)
laser_enabled:=$(arg laser_enabled) laser_enabled:=$(arg laser_enabled)
kinect_enabled:=$(arg kinect_enabled) kinect_enabled:=$(arg kinect_enabled)
urdf_extras:=$(arg urdf_extras)
" /> " />
</launch> </launch>
...@@ -27,12 +27,12 @@ ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT ...@@ -27,12 +27,12 @@ ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
--> -->
<robot xmlns:xacro="http://ros.org/wiki/xacro" name="kinect_camera"> <robot xmlns:xacro="http://ros.org/wiki/xacro" name="zed_camera">
<xacro:arg name="cams_enabled" default="false" /> <xacro:arg name="cams_enabled" default="true" />
<xacro:arg name="depth_enabled" default="true" /> <xacro:arg name="depth_enabled" default="true" />
<xacro:macro name="kinect_camera" params="prefix:=zed robot_namespace:=/"> <xacro:macro name="zed_camera" params="prefix:=zed robot_namespace:=/">
<!-- Create kinect reference frame --> <!-- Create kinect reference frame -->
<!-- Add mesh for zed --> <!-- Add mesh for zed -->
<link name="${prefix}_link"> <link name="${prefix}_link">
...@@ -50,10 +50,17 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. ...@@ -50,10 +50,17 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
</collision> </collision>
</link> </link>
<joint name="${prefix}_optical_joint" type="fixed"> <joint name="${prefix}_frame" type="fixed">
<origin xyz="0 0 0" rpy="0 0 0"/> <origin xyz="0 0 0" rpy="0 0 0"/>
<parent link="${prefix}_link"/> <parent link="${prefix}_link"/>
<child link="${prefix}_depth_optical"/> <child link="${prefix}_frame"/>
</joint>
<joint name="${prefix}_optical_joint" type="fixed">
<!-- <origin xyz="0 0 0" rpy="0 0 0"/> -->
<origin xyz="0 0 0" rpy="-1.5708 0 -1.5708"/>
<parent link="${prefix}_link"/>
<child link="${prefix}_optical_frame"/>
</joint> </joint>
<joint name="${prefix}_cam1_joint" type="fixed"> <joint name="${prefix}_cam1_joint" type="fixed">
...@@ -67,21 +74,14 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. ...@@ -67,21 +74,14 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
<parent link="${prefix}_link"/> <parent link="${prefix}_link"/>
<child link="${prefix}_cam2_optical"/> <child link="${prefix}_cam2_optical"/>
</joint> </joint>
<link name="${prefix}_frame"/>
<joint name="${prefix}_frame_joint" type="fixed"> <link name="${prefix}_optical_frame"/>
<origin xyz="0 0.06 0" rpy="-1.5708 0 -1.5708"/>
<parent link="${prefix}_link"/>
<child link="${prefix}_frame_optical"/>
</joint>
<link name="${prefix}_depth_optical"/>
<link name="${prefix}_cam1_optical"/> <link name="${prefix}_cam1_optical"/>
<link name="${prefix}_cam2_optical"/> <link name="${prefix}_cam2_optical"/>
<link name="${prefix}_frame_optical"/>
<xacro:if value="$(arg depth_enabled)"> <xacro:if value="$(arg depth_enabled)">
<gazebo reference="${prefix}_depth_optical"> <gazebo reference="${prefix}_frame">
<sensor type="depth" name="${prefix}_depth"> <sensor type="depth" name="${prefix}_depth">
<always_on>true</always_on> <always_on>true</always_on>
<update_rate>1.0</update_rate> <update_rate>1.0</update_rate>
...@@ -106,7 +106,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. ...@@ -106,7 +106,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
<pointCloudTopicName>depth/points</pointCloudTopicName> <pointCloudTopicName>depth/points</pointCloudTopicName>
<cameraInfoTopicName>rgb/camera_info</cameraInfoTopicName> <cameraInfoTopicName>rgb/camera_info</cameraInfoTopicName>
<depthImageCameraInfoTopicName>depth/camera_info</depthImageCameraInfoTopicName> <depthImageCameraInfoTopicName>depth/camera_info</depthImageCameraInfoTopicName>
<frameName>${prefix}_frame_optical</frameName> <frameName>${prefix}_optical_frame</frameName>
<distortion_k1>0.0</distortion_k1> <distortion_k1>0.0</distortion_k1>
<distortion_k2>0.0</distortion_k2> <distortion_k2>0.0</distortion_k2>
<distortion_k3>0.0</distortion_k3> <distortion_k3>0.0</distortion_k3>
...@@ -114,7 +114,6 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. ...@@ -114,7 +114,6 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
<distortion_t2>0.0</distortion_t2> <distortion_t2>0.0</distortion_t2>
<pointCloudCutoff>0.5</pointCloudCutoff> <pointCloudCutoff>0.5</pointCloudCutoff>
<pointCloudCutoffMax>25</pointCloudCutoffMax> <pointCloudCutoffMax>25</pointCloudCutoffMax>
<robotNamespace>${robot_namespace}</robotNamespace>
</plugin> </plugin>
</sensor> </sensor>
</gazebo> </gazebo>
...@@ -139,6 +138,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. ...@@ -139,6 +138,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
</camera> </camera>
<plugin name="camera_controller" filename="libgazebo_ros_camera.so"> <plugin name="camera_controller" filename="libgazebo_ros_camera.so">
<cameraName>${prefix}_cam1</cameraName> <cameraName>${prefix}_cam1</cameraName>
<frameName>${prefix}_cam1_optical</frameName>
</plugin> </plugin>
</sensor> </sensor>
</gazebo> </gazebo>
...@@ -161,6 +161,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. ...@@ -161,6 +161,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
</camera> </camera>
<plugin name="camera_controller" filename="libgazebo_ros_camera.so"> <plugin name="camera_controller" filename="libgazebo_ros_camera.so">
<cameraName>${prefix}_cam2</cameraName> <cameraName>${prefix}_cam2</cameraName>
<frameName>${prefix}_cam2_optical</frameName>
</plugin> </plugin>
</sensor> </sensor>
</gazebo> </gazebo>
......
<?xml version="1.0"?> <?xml version="1.0"?>
<robot xmlns:xacro="http://ros.org/wiki/xacro" name="510_mm_sensor_arch"> <robot xmlns:xacro="http://ros.org/wiki/xacro" name="510_mm_sensor_arch">
<xacro:macro name="sensor_arch" params="prefix parent size:=510 *origin"> <xacro:macro name="sensor_arch" params="parent size:=510 *origin">
<!-- Spawn the sensor arch link --> <!-- Spawn the sensor arch link -->
<link name="${prefix}sensor_arch_mount_link"> <link name="sensor_arch_mount_link">
<origin xyz="0 0 0" rpy="0 0 0"/> <origin xyz="0 0 0" rpy="0 0 0"/>
<visual> <visual>
<geometry> <geometry>
...@@ -20,10 +20,10 @@ ...@@ -20,10 +20,10 @@
</link> </link>
<!-- Attach the sensor arch to the top plate --> <!-- Attach the sensor arch to the top plate -->
<joint name="${prefix}sensor_arch_mount" type="fixed"> <joint name="sensor_arch_mount" type="fixed">
<xacro:insert_block name="origin"/> <xacro:insert_block name="origin"/>
<parent link="${parent}"/> <parent link="${parent}"/>
<child link="${prefix}sensor_arch_mount_link"/> <child link="sensor_arch_mount_link"/>
</joint> </joint>
</xacro:macro> </xacro:macro>
......
...@@ -135,13 +135,13 @@ ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSI ...@@ -135,13 +135,13 @@ ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSI
<visual> <visual>
<origin xyz="0 0 0" rpy="0 0 0" /> <origin xyz="0 0 0" rpy="0 0 0" />
<geometry> <geometry>
<mesh filename="package://husky_description/meshes/large_top_plate.dae" /> <mesh filename="package://husky_description/meshes/structure.stl" />
</geometry> </geometry>
</visual> </visual>
<collision> <collision>
<origin xyz="0 0 0" rpy="0 0 0"/> <origin xyz="0 0 0" rpy="0 0 0"/>
<geometry> <geometry>
<mesh filename="package://husky_description/meshes/large_top_plate_collision.stl" /> <mesh filename="package://husky_description/meshes/structure.stl" />
</geometry> </geometry>
</collision> </collision>
</link> </link>
...@@ -170,13 +170,13 @@ ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSI ...@@ -170,13 +170,13 @@ ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSI
<visual> <visual>
<origin xyz="0 0 0" rpy="0 0 0" /> <origin xyz="0 0 0" rpy="0 0 0" />
<geometry> <geometry>
<mesh filename="package://husky_description/meshes/top_plate.dae" /> <mesh filename="package://husky_description/meshes/structure.stl" />
</geometry> </geometry>
</visual> </visual>
<collision> <collision>
<origin xyz="0 0 0" rpy="0 0 0"/> <origin xyz="0 0 0" rpy="0 0 0"/>
<geometry> <geometry>
<mesh filename="package://husky_description/meshes/top_plate.stl" /> <mesh filename="package://husky_description/meshes/structure.stl" />
</geometry> </geometry>
</collision> </collision>
</link> </link>
......
...@@ -25,19 +25,18 @@ ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSI ...@@ -25,19 +25,18 @@ ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSI
--> -->
<robot name="husky" xmlns:xacro="http://ros.org/wiki/xacro"> <robot name="husky" xmlns:xacro="http://ros.org/wiki/xacro">
<xacro:arg name="laser_enabled" default="false" /> <xacro:arg name="laser_enabled" default="true" />
<xacro:arg name="laser_xyz" default="$(optenv HUSKY_LMS1XX_XYZ 0.2206 0.0 0.00635)" /> <xacro:arg name="laser_xyz" default="$(optenv HUSKY_LMS1XX_XYZ 0.2206 0.0 0.00635)" />
<xacro:arg name="laser_rpy" default="$(optenv HUSKY_LMS1XX_RPY 0.0 0.0 0.0)" /> <xacro:arg name="laser_rpy" default="$(optenv HUSKY_LMS1XX_RPY 0.0 0.0 0.0)" />
<xacro:arg name="kinect_enabled" default="false" /> <xacro:arg name="kinect_enabled" default="true" />
<xacro:arg name="kinect_xyz" default="$(optenv HUSKY_KINECT_XYZ 0 0 0)" /> <xacro:arg name="kinect_xyz" default="$(optenv HUSKY_KINECT_XYZ 0 0 0)" />
<xacro:arg name="kinect_rpy" default="$(optenv HUSKY_KINECT_RPY 0 0 0)" /> <xacro:arg name="kinect_rpy" default="$(optenv HUSKY_KINECT_RPY 0 0 0)" />
<xacro:property name="husky_front_bumper_extend" value="$(optenv HUSKY_FRONT_BUMPER_EXTEND 0)" /> <xacro:property name="husky_front_bumper_extend" value="$(optenv HUSKY_FRONT_BUMPER_EXTEND 0)" />
<xacro:property name="husky_rear_bumper_extend" value="$(optenv HUSKY_REAR_BUMPER_EXTEND 0)" /> <xacro:property name="husky_rear_bumper_extend" value="$(optenv HUSKY_REAR_BUMPER_EXTEND 0)" />
<xacro:arg name="robot_namespace" default="/" /> <xacro:arg name="robot_namespace" default="husky" />
<xacro:arg name="urdf_extras" default="empty.urdf" />
<!-- Included URDF/XACRO Files --> <!-- Included URDF/XACRO Files -->
<xacro:include filename="$(find husky_description)/urdf/decorations.urdf.xacro" /> <xacro:include filename="$(find husky_description)/urdf/decorations.urdf.xacro" />
...@@ -159,14 +158,9 @@ ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSI ...@@ -159,14 +158,9 @@ ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSI
</xacro:if> </xacro:if>
<gazebo> <gazebo>
<plugin name="gazebo_ros_control" filename="libgazebo_ros_control.so"> <plugin name="gazebo_ros_control" filename="libgazebo_ros_control.so"/>
<robotNamespace>$(arg robot_namespace)</robotNamespace>
</plugin>
</gazebo>
<gazebo>
<plugin name="imu_controller" filename="libgazebo_ros_imu.so"> <plugin name="imu_controller" filename="libgazebo_ros_imu.so">
<robotNamespace>$(arg robot_namespace)</robotNamespace>
<updateRate>50.0</updateRate> <updateRate>50.0</updateRate>
<bodyName>base_link</bodyName> <bodyName>base_link</bodyName>
<topicName>imu/data</topicName> <topicName>imu/data</topicName>
...@@ -177,11 +171,8 @@ ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSI ...@@ -177,11 +171,8 @@ ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSI
<headingDrift>0.005</headingDrift> <headingDrift>0.005</headingDrift>
<headingGaussianNoise>0.005</headingGaussianNoise> <headingGaussianNoise>0.005</headingGaussianNoise>
</plugin> </plugin>
</gazebo>
<gazebo>
<plugin name="gps_controller" filename="libhector_gazebo_ros_gps.so"> <plugin name="gps_controller" filename="libhector_gazebo_ros_gps.so">
<robotNamespace>$(arg robot_namespace)</robotNamespace>
<updateRate>40</updateRate> <updateRate>40</updateRate>
<bodyName>base_link</bodyName> <bodyName>base_link</bodyName>
<frameId>base_link</frameId> <frameId>base_link</frameId>
...@@ -195,7 +186,5 @@ ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSI ...@@ -195,7 +186,5 @@ ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSI
</plugin> </plugin>
</gazebo> </gazebo>
<!-- Optional custom includes. -->
<!--xacro:include filename="$(arg urdf_extras)" /-->
</robot> </robot>
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment