From: Erik Andresen <erik@vontaene.de>
Date: Mon, 26 Dec 2016 18:24:11 +0000 (+0100)
Subject: fix simulation
X-Git-Url: https://defiant.homedns.org/gitweb/?a=commitdiff_plain;h=47414c87ea933baa2f6b16d30acaca48b7636af2;p=ros_wild_thumper.git

fix simulation
---

diff --git a/launch/gazebo.launch b/launch/gazebo.launch
index da5f559..7580d4e 100644
--- 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 102f98d..3c1880d 100644
--- 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 a5cceb3..d39b379 100644
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 9a46afc..7135eff 100644
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 1bd641e..f84e201 100644
--- 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
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 c0c2ce1..a803e06 100644
--- 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
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>