fix simulation

diff --git a/launch/gazebo.launch b/launch/gazebo.launch
index da5f559469bc5dc51e0885ac6d03c1d5cc312b29..7580d4e0c04fce6be27fbfca94c7505c218f0620 100644 (file)
--- a/launch/gazebo.launch
+++ b/launch/gazebo.launch
@@ -2,6 +2,8 @@
 <launch>
        <arg name="use_imu" default="true"/>
 
+       <node pkg="nodelet" type="nodelet" name="nodelet_manager" args="manager"/>
+
        <!-- Load joint controller configurations from YAML file to parameter server -->
        <rosparam file="$(find wild_thumper)/config/control.yaml" command="load" />
        <rosparam param="/diff_drive_controller/enable_odom_tf" unless="$(arg use_imu)">true</rosparam>
@@ -21,7 +23,7 @@
        <!-- load the controllers -->
        <node name="controller_spawner" pkg="controller_manager" type="spawner" output="screen" ns="/" args="joint_state_controller diff_drive_controller"/>
 
-       <node name="relay_cmd_vel" pkg="topic_tools" type="relay" output="screen" args="/cmd_vel /diff_drive_controller/cmd_vel" />
+       <node name="relay_cmd_vel" pkg="topic_tools" type="relay" output="screen" args="/cmd_vel_out /diff_drive_controller/cmd_vel" />
        <node name="relay_odom" pkg="topic_tools" type="relay" output="screen" args="/diff_drive_controller/odom /odom" />
 
        <node pkg="depthimage_to_laserscan" type="depthimage_to_laserscan" name="depthimage_to_laserscan" output="screen">
@@ -37,4 +39,9 @@
                <param name="debug" value="true"/>
                <param name="sensor_timeout" value="2.0"/>
        </node>
+
+       <include file="$(find yocs_cmd_vel_mux)/launch/cmd_vel_mux.launch">
+               <arg name="config_file" value="$(find wild_thumper)/config/cmd_vel_mux.yaml" />
+               <arg name="nodelet_manager_name" value="nodelet_manager" />
+       </include>
 </launch>

diff --git a/launch/move_base.launch b/launch/move_base.launch
index 102f98dadb2639e63b42d40eadbc31c4e7a0f826..3c1880d8d4ae09f3cb55a9c49ae91ed4d6db21db 100644 (file)
--- a/launch/move_base.launch
+++ b/launch/move_base.launch
@@ -17,7 +17,7 @@
        <node pkg="gmapping" type="slam_gmapping" name="slap_gmapping" if="$(arg slam_gmapping)">
                <param name="scan" value="scan" />
                <param name="delta" value="0.01" />
-               <param name="map_update_interval" value="30" />
+               <param name="map_update_interval" value="5.0" />
                <param name="linearUpdate" value="0.5" />
                <param name="angularUpdate" value="0.25" />
                <param name="xmin" value="-5.0"/>
@@ -25,6 +25,11 @@
                <param name="xmax" value="5.0"/>
                <param name="ymax" value="5.0"/>
                <param name="odom_frame" value="odom"/>
+               <param name="maxUrange" value="6.0"/>
+               <param name="maxRange" value="8.0"/>
+               <param name="minimumScore" value="200"/>
+               <param name="linearUpdate" value="0.5"/>
+               <param name="particles" value="80"/>
        </node>
 
        <node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen">

diff --git a/meshes/wheel_left.stl b/meshes/wheel_left.stl
index a5cceb3b077526b41745d88fff04c9ccaa6a865b..d39b379b1be4eaba2eb48c5b5513b47d706d1781 100644 (file)
Binary files a/meshes/wheel_left.stl and b/meshes/wheel_left.stl differ

diff --git a/meshes/wheel_right.stl b/meshes/wheel_right.stl
index 9a46afc66e1e0cfbd720435f574ff5dbe80a255f..7135eff68ffb1e82cafdebb66d397d41022bc04b 100644 (file)
Binary files a/meshes/wheel_right.stl and b/meshes/wheel_right.stl differ

diff --git a/urdf/wild_thumper.urdf.xacro b/urdf/wild_thumper.urdf.xacro
index 1bd641e23a0a12bc4f9d22e76627abc646ea88f5..f84e201314ddb72307d1042b9f5cee7f38d5c7b5 100644 (file)
--- a/urdf/wild_thumper.urdf.xacro
+++ b/urdf/wild_thumper.urdf.xacro
@@ -21,11 +21,13 @@
                        </geometry>
                </collision>
                <visual>
+                       <origin xyz="-0.00275 0 0" rpy="0 0 0"/>
                        <geometry>
                                <mesh filename="package://wild_thumper/meshes/wild_thumper_4wd.stl" />
                        </geometry>
                </visual>
                <inertial>
+                       <origin xyz="-0.00275 0 -0.02" rpy="0 0 0"/>
                        <mass value="2.5"/>
                        <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
                </inertial>
@@ -161,9 +163,18 @@
                        <inertial>
                                <origin xyz="0 0 0" rpy="0 0 0"/>
                                <mass value="0.1"/>
-                               <inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="0" izz="0.01"/>
+                               <inertia ixx="0.005" ixy="0" ixz="0" iyy="0.005" iyz="0" izz="0.005"/>
                        </inertial>
                </link>
+
+               <gazebo reference="${pos}_${side}_wheel">
+                       <mu1 value="1.0"/>
+                       <mu2 value="1.0"/>
+                       <kp value="1000000.0" />
+                       <kd value="1.0" />
+                       <fdir1 value="1 0 0"/>
+               </gazebo>
+
                <joint name="${pos}_${side}_wheel_joint" type="continuous">
                        <parent link="base_link"/>
                        <child link="${pos}_${side}_wheel"/>
@@ -177,16 +188,15 @@
                                <hardwareInterface>VelocityJointInterface</hardwareInterface>
                        </joint>
                        <actuator name="{pos}_${side}_wheel_motor">
-                               <hardwareInterface>VelocityJointInterface</hardwareInterface>
                                <mechanicalReduction>1</mechanicalReduction>
                        </actuator>
                </transmission>
        </xacro:macro>
 
-       <xacro:wheel pos="rear" side="left"  xyz="-0.06398 0.114 -0.022446" rpy="0 0 0" />
-       <xacro:wheel pos="rear" side="right" xyz="-0.07397 -0.114 -0.022446" rpy="0 0 0" />
-       <xacro:wheel pos="front" side="left"  xyz="0.07602 0.114 -0.022446" rpy="0 0 0" />
-       <xacro:wheel pos="front" side="right" xyz="0.07397 -0.114 -0.022446" rpy="0 0 0" />
+       <xacro:wheel pos="rear" side="left"   xyz="-0.07525  0.12 -0.025" rpy="${-5*PI/180} 0 0" />
+       <xacro:wheel pos="rear" side="right"  xyz="-0.07525 -0.12 -0.025" rpy="${5*PI/180} 0 0" />
+       <xacro:wheel pos="front" side="left"  xyz=" 0.07525  0.12 -0.025" rpy="${-5*PI/180} 0 0" />
+       <xacro:wheel pos="front" side="right" xyz=" 0.07525 -0.12 -0.025" rpy="${5*PI/180} 0 0" />
 
        <gazebo>
                <plugin name="gazebo_ros_control" filename="libgazebo_ros_control.so">
@@ -203,6 +213,7 @@
                        <frameName>base_imu_link</frameName>
                        <xyzOffsets>0 0 0</xyzOffsets>
                        <rpyOffset>0 0 0</rpyOffset>
+                       <serviceName>/default_imu</serviceName>
                </plugin>
        </gazebo>
 
@@ -246,8 +257,8 @@
                </gazebo>
        </xacro:macro>
 
-       <xacro:range_sensor name="sonar_forward" ros_topic="range_forward" update_rate="10" minRange="0.04" maxRange="6" fov="${30*PI/180}" radiation="ultrasound" />
-       <xacro:range_sensor name="sonar_backward" ros_topic="range_backward" update_rate="10" minRange="0.04" maxRange="6" fov="${30*PI/180}" radiation="ultrasound" />
+       <xacro:range_sensor name="sonar_forward" ros_topic="range_forward" update_rate="10" minRange="0.04" maxRange="0.5" fov="${20*PI/180}" radiation="ultrasound" />
+       <xacro:range_sensor name="sonar_backward" ros_topic="range_backward" update_rate="10" minRange="0.04" maxRange="0.5" fov="${20*PI/180}" radiation="ultrasound" />
        <xacro:range_sensor name="ir_left" ros_topic="range_left" update_rate="10" minRange="0.04" maxRange="0.3" fov="${5*PI/180}" radiation="infrared" />
        <xacro:range_sensor name="ir_right" ros_topic="range_right" update_rate="10" minRange="0.04" maxRange="0.3" fov="${5*PI/180}" radiation="infrared" />
 </robot>

diff --git a/worlds/gas_station.world b/worlds/gas_station.world
new file mode 100644 (file)
index 0000000..c0c2ce1
--- /dev/null
+++ b/worlds/gas_station.world
@@ -0,0 +1,20 @@
+<?xml version="1.0" ?>
+<sdf version="1.4" >
+       <world name="default" >
+               <include>
+                       <uri>model://ground_plane</uri>
+               </include>
+               <include>
+                       <uri>model://sun</uri>
+               </include>
+               <include>
+                       <uri>model://gas_station</uri>
+                       <name>gas_station</name>
+                       <pose>-2.0 7.0 0 0 0 0</pose>
+               </include>
+               <physics type='ode'>
+                       <max_step_size>0.02</max_step_size>
+                       <real_time_update_rate>50</real_time_update_rate>
+               </physics>
+       </world>
+</sdf>

diff --git a/worlds/test.world b/worlds/test.world
index c0c2ce177f08719bd897cb7d86cdaea3f758712e..a803e061b0770f026b3bc6dc6a1da5f5f3ec4d5e 100644 (file)
--- a/worlds/test.world
+++ b/worlds/test.world
@@ -8,9 +8,9 @@
                        <uri>model://sun</uri>
                </include>
                <include>
-                       <uri>model://gas_station</uri>
-                       <name>gas_station</name>
-                       <pose>-2.0 7.0 0 0 0 0</pose>
+                       <uri>model://regerstr</uri>
+                       <name>regerstr</name>
+                       <pose>-5.0 5.0 0 1.5708 0 0</pose>
                </include>
                <physics type='ode'>
                        <max_step_size>0.02</max_step_size>

diff --git a/worlds/willowgarage.world b/worlds/willowgarage.world
new file mode 100644 (file)
index 0000000..e95b160
--- /dev/null
+++ b/worlds/willowgarage.world
@@ -0,0 +1,20 @@
+<?xml version="1.0" ?>
+<sdf version="1.4" >
+       <world name="default" >
+               <include>
+                       <uri>model://ground_plane</uri>
+               </include>
+               <include>
+                       <uri>model://sun</uri>
+               </include>
+               <include>
+                       <uri>model://willowgarage</uri>
+                       <name>willowgarage</name>
+                       <pose>-17.0 -9.0 0 0 0 0</pose>
+               </include>
+               <physics type='ode'>
+                       <max_step_size>0.02</max_step_size>
+                       <real_time_update_rate>50</real_time_update_rate>
+               </physics>
+       </world>
+</sdf>