From fef837ff325499eccd07828f2f1ce104727d37db Mon Sep 17 00:00:00 2001
From: Erik Andresen <erik@vontaene.de>
Date: Thu, 3 Sep 2015 19:53:14 +0200
Subject: [PATCH 1/1] simulation/gazebo: added controller for imu, range
 sensors and diff drive

---
 config/control.yaml          |  41 ++++++++++++++
 launch/gazebo.launch         |  19 +++++++
 scripts/wt_node.py           |  11 ++--
 urdf/wild_thumper.urdf.xacro | 105 +++++++++++++++++++++++------------
 worlds/test.world            |   4 ++
 5 files changed, 142 insertions(+), 38 deletions(-)
 create mode 100644 config/control.yaml

diff --git a/config/control.yaml b/config/control.yaml
new file mode 100644
index 0000000..648218d
--- /dev/null
+++ b/config/control.yaml
@@ -0,0 +1,41 @@
+joint_state_controller:
+  type: "joint_state_controller/JointStateController"
+  publish_rate: 20
+
+diff_drive_controller:
+  type: "diff_drive_controller/DiffDriveController"
+  left_wheel: ['front_left_wheel_joint', 'rear_left_wheel_joint']
+  right_wheel: ['front_right_wheel_joint', 'rear_right_wheel_joint']
+  publish_rate: 20
+  pose_covariance_diagonal:  [0.001, 0.001, 1000000.0, 1000000.0, 1000000.0, 0.03]
+  twist_covariance_diagonal: [0.001, 0.001, 1000000.0, 1000000.0, 1000000.0, 0.03]
+  cmd_vel_timeout: 1000000000
+
+  # Base frame_id
+  base_frame_id: base_footprint
+
+  # Odometry fused with IMU is published by robot_localization, so
+  # no need to publish a TF based on encoders alone.
+  enable_odom_tf: false
+
+  # Hardware provides wheel velocities
+  estimate_velocity_from_position: false
+
+  # Wheel separation and radius multipliers
+  wheel_separation_multiplier: 1.0 # default: 1.0
+  wheel_radius_multiplier    : 1.0 # default: 1.0
+
+  # Velocity and acceleration limits
+  # Whenever a min_* is unspecified, default to -max_*
+  linear:
+    x:
+      has_velocity_limits    : true
+      max_velocity           : 1.5   # m/s
+      has_acceleration_limits: true
+      max_acceleration       : 2.0   # m/s^2
+  angular:
+    z:
+      has_velocity_limits    : true
+      max_velocity           : 3.0   # rad/s
+      has_acceleration_limits: true
+      max_acceleration       : 3.0   # rad/s^2
diff --git a/launch/gazebo.launch b/launch/gazebo.launch
index 219529a..b93b250 100644
--- a/launch/gazebo.launch
+++ b/launch/gazebo.launch
@@ -1,5 +1,8 @@
 <?xml version="1.0"?>
 <launch>
+	<!-- Load joint controller configurations from YAML file to parameter server -->
+	<rosparam file="$(find wild_thumper)/config/control.yaml" command="load" />
+
 	<include file="$(find gazebo_ros)/launch/empty_world.launch" >
 		<arg name="world_name" value="$(find wild_thumper)/worlds/test.world" />
 		<arg name="gui" value="false" />
@@ -12,7 +15,23 @@
 	<!-- Spawn a robot into Gazebo -->
 	<node name="spawn_urdf" pkg="gazebo_ros" type="spawn_model" args="-param robot_description -urdf -model wild_thumper" output="screen" />
 
+	<!-- 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_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">
 		<remap from="/image" to="/camera/depth/image_raw"/>
         </node>
+
+	<node pkg="robot_pose_ekf" type="robot_pose_ekf" name="robot_pose_ekf" output="screen">
+		<remap from="imu_data" to="imu"/>
+		<remap from="robot_pose_ekf/odom_combined" to="odom_combined"/>
+		<param name="output_frame" value="odom"/>
+		<param name="freq" value="20.0"/>
+		<param name="vo_used" value="false"/>
+		<param name="debug" value="true"/>
+		<param name="sensor_timeout" value="2.0"/>
+	</node>
 </launch>
diff --git a/scripts/wt_node.py b/scripts/wt_node.py
index 72c0412..96d0768 100755
--- a/scripts/wt_node.py
+++ b/scripts/wt_node.py
@@ -39,15 +39,18 @@ class MoveBase:
 		rate = rospy.Rate(20.0)
 		reset_val = self.get_reset()
 		rospy.loginfo("Reset Status: 0x%x" % reset_val)
+		i = 0
 		while not rospy.is_shutdown():
 			#print struct.unpack(">B", i2c_read_reg(0x50, 0xA2, 1))[0] # count test
 			self.get_tle_err()
 			self.get_odom()
 			self.get_voltage()
-			self.get_dist_forward()
-			self.get_dist_backward()
-			self.get_dist_left()
-			self.get_dist_right()
+			if i % 2:
+				self.get_dist_forward()
+				self.get_dist_backward()
+				self.get_dist_left()
+				self.get_dist_right()
+			i+=1
 			rate.sleep()
 
 	def set_motor_handicap(self, front, aft): # percent
diff --git a/urdf/wild_thumper.urdf.xacro b/urdf/wild_thumper.urdf.xacro
index cf2c672..733a51d 100644
--- a/urdf/wild_thumper.urdf.xacro
+++ b/urdf/wild_thumper.urdf.xacro
@@ -26,7 +26,7 @@
 			</geometry>
 		</visual>
 		<inertial>
-			<mass value="1.9"/>
+			<mass value="2.5"/>
 			<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
 		</inertial>
 	</link>
@@ -170,47 +170,84 @@
 			<axis xyz="0 1 0"/>
 			<origin xyz="${xyz}" rpy="${rpy}"/>
 		</joint>
-	</xacro:macro>
 
-	<gazebo reference="front_left_wheel">
-		<dampingFactor>0.001</dampingFactor>
-	</gazebo>
-	<gazebo reference="front_right_wheel">
-		<dampingFactor>0.001</dampingFactor>
-	</gazebo>
+		<transmission name="${pos}_${side}_wheel_trans" type="SimpleTransmission">
+			<type>transmission_interface/SimpleTransmission</type>
+			<joint name="${pos}_${side}_wheel_joint">
+				<hardwareInterface>VelocityJointInterface</hardwareInterface>
+			</joint>
+			<actuator name="{pos}_${side}_wheel_motor">
+				<hardwareInterface>VelocityJointInterface</hardwareInterface>
+				<mechanicalReduction>1</mechanicalReduction>
+			</actuator>
+		</transmission>
+	</xacro:macro>
 
-	<xacro:wheel pos="aft" side="left"  xyz="-0.06398 0.11407 -0.022446" rpy="0 0 0" />
-	<xacro:wheel pos="aft" side="right" xyz="-0.07397 -0.11408 -0.022446" rpy="0 0 0" />
-	<xacro:wheel pos="front" side="left"  xyz="0.07602 0.11407 -0.022446" rpy="0 0 0" />
-	<xacro:wheel pos="front" side="right" xyz="0.07397 -0.11408 -0.022446" rpy="0 0 0" />
+	<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" />
 
 	<gazebo>
-		<plugin name="joint_state_publisher" filename="libgazebo_ros_joint_state_publisher.so">
+		<plugin name="gazebo_ros_control" filename="libgazebo_ros_control.so">
 			<robotNamespace>/</robotNamespace>
-			<jointName>aft_left_wheel_joint, aft_right_wheel_joint, front_left_wheel_joint, front_right_wheel_joint</jointName>
-			<updateRate>10.0</updateRate>
-			<alwaysOn>true</alwaysOn>
+			<robotSimType>gazebo_ros_control/DefaultRobotHWSim</robotSimType>
 		</plugin>
 
-		<plugin name="differential_drive_controller" filename="libgazebo_ros_diff_drive.so">
-			<robotNamespace>/</robotNamespace>
+		<plugin name="imu_controller" filename="libgazebo_ros_imu.so">
 			<alwaysOn>true</alwaysOn>
-			<updateRate>10.0</updateRate>
-			<leftJoint>aft_right_wheel_joint</leftJoint>
-			<rightJoint>aft_left_wheel_joint</rightJoint>
-			<wheelSeparation>0.252</wheelSeparation>
-			<wheelDiameter>0.12</wheelDiameter>
-			<wheelTorque>5.0</wheelTorque>
-			<commandTopic>cmd_vel</commandTopic>
-			<odometryTopic>odom</odometryTopic>
-			<odometryFrame>odom</odometryFrame>
-			<robotBaseFrame>base_footprint</robotBaseFrame>
-			<rosDebugLevel>Info</rosDebugLevel>
-			<publishWheelTF>false</publishWheelTF>
-			<publishWheelJointState>true</publishWheelJointState>
-			<wheelAcceleration>0.0</wheelAcceleration>
-			<odometrySource>world</odometrySource>
-			<publishTf>true</publishTf>
+			<updateRate>20.0</updateRate>
+			<bodyName>base_imu_link</bodyName>
+			<topicName>imu</topicName>
+			<gaussianNoise>0.05</gaussianNoise>
+			<frameName>base_imu_link</frameName>
+			<xyzOffsets>0 0 0</xyzOffsets>
+			<rpyOffset>0 0 0</rpyOffset>
 		</plugin>
 	</gazebo>
+
+	<xacro:macro name="range_sensor" params="name ros_topic update_rate minRange maxRange fov radiation">
+		<gazebo reference="${name}">
+			<sensor type="ray" name="${name}_sensor">
+				<pose>0 0 0 0 0 0</pose>
+				<update_rate>${update_rate}</update_rate>
+				<visualize>false</visualize>
+				<ray>
+					<scan>
+						<horizontal>
+							<samples>5</samples>
+							<resolution>1</resolution>
+							<min_angle>-${fov/2}</min_angle>
+							<max_angle>${fov/2}</max_angle>
+						</horizontal>
+						<vertical>
+							<samples>5</samples>
+							<resolution>1</resolution>
+							<min_angle>-${fov/2}</min_angle>
+							<max_angle>${fov/2}</max_angle>
+						</vertical>
+					</scan>
+					<range>
+						<min>${minRange}</min>
+						<max>${maxRange}</max>
+						<resolution>0.01</resolution>
+					</range>
+				</ray>
+				<plugin filename="libgazebo_ros_range.so" name="gazebo_ros_range">
+					<gaussianNoise>0.005</gaussianNoise>
+					<alwaysOn>true</alwaysOn>
+					<updateRate>${update_rate}</updateRate>
+					<topicName>${ros_topic}</topicName>
+					<frameName>${name}</frameName>
+					<fov>${fov}</fov>
+					<radiation>${radiation}</radiation>
+				</plugin>
+			</sensor>
+		</gazebo>
+	</xacro:macro>
+
+	<xacro:range_sensor name="sonar_forward" ros_topic="range_forward" update_rate="10" minRange="0.04" maxRange="6" fov="${60*PI/180}" radiation="ultrasound" />
+	<xacro:range_sensor name="sonar_backward" ros_topic="range_backward" update_rate="10" minRange="0.04" maxRange="6" fov="${60*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/test.world b/worlds/test.world
index 8fc420c..c0c2ce1 100644
--- a/worlds/test.world
+++ b/worlds/test.world
@@ -12,5 +12,9 @@
 			<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>
-- 
2.39.2