Skip to content
GitLab
Unverified Commit 09d001a6 authored by Simon Thomas's avatar Simon Thomas
Browse files

First version for ukf with all sensors

parent eb869bad
Hide whitespace changes
Inline Side-by-side
husky_control/config/localization.yaml
View file @ 09d001a6
odom_frame: odom
base_link_frame: base_link
world_frame: odom
world_frame: map
two_d_mode: true
......@@ -15,6 +15,24 @@ odom0_config: [false, false, false,
odom0_differential: false
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_config: [false, false, false,
true, true, true,
......
husky_control/config/localization_odom.yaml 0 → 100644
View file @ 09d001a6
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
husky_control/launch/control.launch
View file @ 09d001a6
......@@ -5,12 +5,11 @@
<arg name="simulation" default="$(optenv ROBOT_SIMULATION false)"/>
<arg name="robot_namespace" default="$(optenv ROBOT_NAMESPACE robot)"/>
<arg name="config_extras"
default="$(eval optenv('HUSKY_CONFIG_EXTRAS', find('husky_control') + '/config/empty.yaml'))"/>
# <arg name="config_extras"
default="$(eval optenv('HUSKY_CONFIG_EXTRAS', find('husky_control') + '/config/empty.yaml'))"/>
<arg name="laser_enabled" default="$(optenv HUSKY_LMS1XX_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)">
<arg name="gazebo_interface" value="$(find husky_control)/config/gazebo_interface.yaml" />
......@@ -23,7 +22,6 @@
<include file="$(find husky_description)/launch/description.launch" >
<arg name="laser_enabled" default="$(arg laser_enabled)"/>
<arg name="kinect_enabled" default="$(arg kinect_enabled)"/>
<arg name="urdf_extras" default="$(arg urdf_extras)"/>
</include>
<!-- Load controller configuration -->
......@@ -33,14 +31,14 @@
<node name="base_controller_spawner" pkg="controller_manager" type="spawner"
args="husky_joint_publisher husky_velocity_controller"/>
<!-- Start EKF for localization -->
<node pkg="robot_localization" type="ekf_localization_node" name="ekf_localization">
<!-- Start UKF for localization -->
<node pkg="robot_localization" type="ukf_localization_node" name="ukf_localization">
<rosparam command="load" file="$(find husky_control)/config/localization.yaml" />
</node>
<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">
<rosparam command="load" file="$(find husky_control)/config/twist_mux.yaml" />
......@@ -48,6 +46,5 @@
</node>
<!-- Override the default control parameters, see config/empty.yaml for default. -->
<rosparam command="load" file="$(arg config_extras)" />
</launch>
husky_description/launch/description.launch
View file @ 09d001a6
......@@ -25,17 +25,14 @@ ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSI
-->
<launch>
<arg name="robot_namespace" default="/"/>
<arg name="robot_namespace" default="husky"/>
<arg name="laser_enabled" default="$(optenv HUSKY_LMS1XX_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'
--inorder
<param name="robot_description" command="$(find xacro)/xacro '$(find erl_simulations)/models/husky.urdf.xacro'
robot_namespace:=$(arg robot_namespace)
laser_enabled:=$(arg laser_enabled)
kinect_enabled:=$(arg kinect_enabled)
urdf_extras:=$(arg urdf_extras)
" />
</launch>
husky_description/urdf/accessories/kinect_camera.urdf.xacro
View file @ 09d001a6
......@@ -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.
-->
<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:macro name="kinect_camera" params="prefix:=zed robot_namespace:=/">
<xacro:macro name="zed_camera" params="prefix:=zed robot_namespace:=/">
<!-- Create kinect reference frame -->
<!-- Add mesh for zed -->
<link name="${prefix}_link">
......@@ -50,10 +50,17 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
</collision>
</link>
<joint name="${prefix}_optical_joint" type="fixed">
<joint name="${prefix}_frame" type="fixed">
<origin xyz="0 0 0" rpy="0 0 0"/>
<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 name="${prefix}_cam1_joint" type="fixed">
......@@ -67,21 +74,14 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
<parent link="${prefix}_link"/>
<child link="${prefix}_cam2_optical"/>
</joint>
<joint name="${prefix}_frame_joint" type="fixed">
<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}_frame"/>
<link name="${prefix}_optical_frame"/>
<link name="${prefix}_cam1_optical"/>
<link name="${prefix}_cam2_optical"/>
<link name="${prefix}_frame_optical"/>
<xacro:if value="$(arg depth_enabled)">
<gazebo reference="${prefix}_depth_optical">
<gazebo reference="${prefix}_frame">
<sensor type="depth" name="${prefix}_depth">
<always_on>true</always_on>
<update_rate>1.0</update_rate>
......@@ -106,7 +106,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
<pointCloudTopicName>depth/points</pointCloudTopicName>
<cameraInfoTopicName>rgb/camera_info</cameraInfoTopicName>
<depthImageCameraInfoTopicName>depth/camera_info</depthImageCameraInfoTopicName>
<frameName>${prefix}_frame_optical</frameName>
<frameName>${prefix}_optical_frame</frameName>
<distortion_k1>0.0</distortion_k1>
<distortion_k2>0.0</distortion_k2>
<distortion_k3>0.0</distortion_k3>
......@@ -114,7 +114,6 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
<distortion_t2>0.0</distortion_t2>
<pointCloudCutoff>0.5</pointCloudCutoff>
<pointCloudCutoffMax>25</pointCloudCutoffMax>
<robotNamespace>${robot_namespace}</robotNamespace>
</plugin>
</sensor>
</gazebo>
......@@ -139,6 +138,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
</camera>
<plugin name="camera_controller" filename="libgazebo_ros_camera.so">
<cameraName>${prefix}_cam1</cameraName>
<frameName>${prefix}_cam1_optical</frameName>
</plugin>
</sensor>
</gazebo>
......@@ -161,6 +161,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
</camera>
<plugin name="camera_controller" filename="libgazebo_ros_camera.so">
<cameraName>${prefix}_cam2</cameraName>
<frameName>${prefix}_cam2_optical</frameName>
</plugin>
</sensor>
</gazebo>
......
husky_description/urdf/accessories/sensor_arch.urdf.xacro
View file @ 09d001a6
<?xml version="1.0"?>
<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 -->
<link name="${prefix}sensor_arch_mount_link">
<link name="sensor_arch_mount_link">
<origin xyz="0 0 0" rpy="0 0 0"/>
<visual>
<geometry>
......@@ -20,10 +20,10 @@
</link>
<!-- 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"/>
<parent link="${parent}"/>
<child link="${prefix}sensor_arch_mount_link"/>
<child link="sensor_arch_mount_link"/>
</joint>
</xacro:macro>
......
husky_description/urdf/decorations.urdf.xacro
View file @ 09d001a6
......@@ -135,13 +135,13 @@ ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSI
<visual>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<mesh filename="package://husky_description/meshes/large_top_plate.dae" />
<mesh filename="package://husky_description/meshes/structure.stl" />
</geometry>
</visual>
<collision>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<mesh filename="package://husky_description/meshes/large_top_plate_collision.stl" />
<mesh filename="package://husky_description/meshes/structure.stl" />
</geometry>
</collision>
</link>
......@@ -170,13 +170,13 @@ ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSI
<visual>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<mesh filename="package://husky_description/meshes/top_plate.dae" />
<mesh filename="package://husky_description/meshes/structure.stl" />
</geometry>
</visual>
<collision>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<mesh filename="package://husky_description/meshes/top_plate.stl" />
<mesh filename="package://husky_description/meshes/structure.stl" />
</geometry>
</collision>
</link>
......
husky_description/urdf/husky.urdf.xacro
View file @ 09d001a6
......@@ -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">
<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_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_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_rear_bumper_extend" value="$(optenv HUSKY_REAR_BUMPER_EXTEND 0)" />
<xacro:arg name="robot_namespace" default="/" />
<xacro:arg name="urdf_extras" default="empty.urdf" />
<xacro:arg name="robot_namespace" default="husky" />
<!-- Included URDF/XACRO Files -->
<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
</xacro:if>
<gazebo>
<plugin name="gazebo_ros_control" filename="libgazebo_ros_control.so">
<robotNamespace>$(arg robot_namespace)</robotNamespace>
</plugin>
</gazebo>
<gazebo>
<plugin name="gazebo_ros_control" filename="libgazebo_ros_control.so"/>
<plugin name="imu_controller" filename="libgazebo_ros_imu.so">
<robotNamespace>$(arg robot_namespace)</robotNamespace>
<updateRate>50.0</updateRate>
<bodyName>base_link</bodyName>
<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
<headingDrift>0.005</headingDrift>
<headingGaussianNoise>0.005</headingGaussianNoise>
</plugin>
</gazebo>
<gazebo>
<plugin name="gps_controller" filename="libhector_gazebo_ros_gps.so">
<robotNamespace>$(arg robot_namespace)</robotNamespace>
<updateRate>40</updateRate>
<bodyName>base_link</bodyName>
<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
</plugin>
</gazebo>
<!-- Optional custom includes. -->
<!--xacro:include filename="$(arg urdf_extras)" /-->
</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