simulation/gazebo: added controller for imu, range sensors and diff

drive

diff --git a/config/control.yaml b/config/control.yaml
new file mode 100644 (file)
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 219529a0e9e115ba42d5753fff2c09486329ada4..b93b250452b10b7eba29fede1a502f1ddc2c4f4a 100644 (file)
--- a/launch/gazebo.launch
+++ b/launch/gazebo.launch
@@ -1,5 +1,8 @@
 <?xml version="1.0"?>
 <launch>
 <?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" />
        <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" />
 
        <!-- 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="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>
 </launch>

diff --git a/scripts/wt_node.py b/scripts/wt_node.py
index 72c04123a4428cf4bd4c7e362ff7a0ed6cc2c806..96d07689809932aee4722db7e37e4627e25615d8 100755 (executable)
--- 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)
                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()
                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
                        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 cf2c67240b9edfe7806f2ac61bb31149f23216da..733a51d2e0b0267ab60bdb75c834ad1e2920d4c3 100644 (file)
--- a/urdf/wild_thumper.urdf.xacro
+++ b/urdf/wild_thumper.urdf.xacro
@@ -26,7 +26,7 @@
                        </geometry>
                </visual>
                <inertial>
                        </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>
                        <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>
                        <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>
 
        <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>
                        <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>
 

-               <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>
                        <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>
                </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>
 </robot>

diff --git a/worlds/test.world b/worlds/test.world
index 8fc420c990d671ceeafb2d0ed1357d2a013f4ea2..c0c2ce177f08719bd897cb7d86cdaea3f758712e 100644 (file)
--- 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>
                        <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>
        </world>
 </sdf>