added umbmark.py, tested with simulation
authorErik Andresen <erik@vontaene.de>
Sun, 6 Sep 2015 08:51:32 +0000 (10:51 +0200)
committerErik Andresen <erik@vontaene.de>
Sun, 6 Sep 2015 08:51:32 +0000 (10:51 +0200)
config/control.yaml
config/global_costmap_params_odom.yaml [new file with mode: 0644]
launch/gazebo.launch
launch/move_base.launch
scripts/umbmark.py [new file with mode: 0755]
scripts/waypoint.py

index 648218d..97d83a5 100644 (file)
@@ -22,8 +22,8 @@ diff_drive_controller:
   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
+  wheel_separation_multiplier: 1.4696 # default: 1.0
+  wheel_radius_multiplier    : 0.98346 # default: 1.0
 
   # Velocity and acceleration limits
   # Whenever a min_* is unspecified, default to -max_*
diff --git a/config/global_costmap_params_odom.yaml b/config/global_costmap_params_odom.yaml
new file mode 100644 (file)
index 0000000..3dd3c90
--- /dev/null
@@ -0,0 +1,11 @@
+global_costmap:
+  global_frame: odom
+  robot_base_frame: base_footprint
+  update_frequency: 5.0
+  static_map: true
+  rolling_window: true
+  width: 10.0
+  height: 10.0
+  plugins:
+  - {name: obstacle_layer, type: 'costmap_2d::ObstacleLayer'}
+  - {name: inflation_layer, type: 'costmap_2d::InflationLayer'}
index b93b250..da5f559 100644 (file)
@@ -1,7 +1,10 @@
 <?xml version="1.0"?>
 <launch>
+       <arg name="use_imu" default="true"/>
+
        <!-- 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>
 
        <include file="$(find gazebo_ros)/launch/empty_world.launch" >
                <arg name="world_name" value="$(find wild_thumper)/worlds/test.world" />
@@ -25,7 +28,7 @@
                <remap from="/image" to="/camera/depth/image_raw"/>
         </node>
 
-       <node pkg="robot_pose_ekf" type="robot_pose_ekf" name="robot_pose_ekf" output="screen">
+       <node pkg="robot_pose_ekf" type="robot_pose_ekf" name="robot_pose_ekf" output="screen" if="$(arg use_imu)">
                <remap from="imu_data" to="imu"/>
                <remap from="robot_pose_ekf/odom_combined" to="odom_combined"/>
                <param name="output_frame" value="odom"/>
index b6924bd..441dbea 100644 (file)
@@ -2,13 +2,16 @@
 <launch>
        <arg name="slam_gmapping" default="false" />
        <arg name="map_file" default="$(find wild_thumper)/data/map.yaml" />
+       <arg name="nomap" default="false" />
 
        <!-- Run the map server -->
        <node name="map_server" pkg="map_server" type="map_server" args="$(arg map_file)" unless="$(arg slam_gmapping)" />
 
        <!-- Run AMCL -->
-       <include file="$(find amcl)/examples/amcl_diff.launch" unless="$(arg slam_gmapping)" />
-       <param name="amcl/base_frame_id" value="base_footprint" />
+       <group unless="$(arg nomap)">
+               <include file="$(find amcl)/examples/amcl_diff.launch" unless="$(arg slam_gmapping)" />
+               <param name="amcl/base_frame_id" value="base_footprint" />
+       </group>
 
        <node pkg="gmapping" type="slam_gmapping" name="slap_gmapping" output="screen" if="$(arg slam_gmapping)">
                <param name="scan" value="scan" />
@@ -26,7 +29,8 @@
        <node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen">
                <rosparam file="$(find wild_thumper)/config/costmap_common_params.yaml" command="load" ns="global_costmap" />
                <rosparam file="$(find wild_thumper)/config/costmap_common_params.yaml" command="load" ns="local_costmap" />
-               <rosparam file="$(find wild_thumper)/config/global_costmap_params.yaml" command="load" />
+               <rosparam file="$(find wild_thumper)/config/global_costmap_params.yaml" command="load" unless="$(arg nomap)" />
+               <rosparam file="$(find wild_thumper)/config/global_costmap_params_odom.yaml" command="load" if="$(arg nomap)" />
                <rosparam file="$(find wild_thumper)/config/local_costmap_params.yaml" command="load" />
                <rosparam file="$(find wild_thumper)/config/base_local_planner_params.yaml" command="load" />
        </node>
diff --git a/scripts/umbmark.py b/scripts/umbmark.py
new file mode 100755 (executable)
index 0000000..d103b5c
--- /dev/null
@@ -0,0 +1,98 @@
+#!/usr/bin/env python
+# -*- coding: iso-8859-15 -*-
+#
+# Gazebo Position: rostopic echo -n 1 /gazebo/model_states
+#
+# x_err = x_real - x_odom
+# y_err = y_real - y_odom
+# phi_err = phi_real - phi_odom
+#
+# x_cg_cw/ccw: avg x_err
+# y_cg_cw/ccw: avg y_err
+#
+# r_cg_cw = sqrt(x_cg_cw**2 + y_cg_cw**2)
+# r_cg_ccw = sqrt(x_cg_ccw**2 + y_cg_ccw**2)
+#
+# L: length (2)
+# D(l/r): Wheel diameter left/right
+# b: wheelbase
+#
+# Wheel diameter correction:
+# beta = (y_cg_cw + y_cg_ccw)/(-4*L)
+# R = (L/2)/sin(beta/2)
+# Ed = Dr/Dl*(R+b/2)/(R-b/2)
+# 
+# Wheelbase correction:
+# alpha = (y_cg_cw - y_cg_ccw)/(-4*L) * 180/pi
+# Eb = (90)/(90-alpha)
+
+import sys
+import rospy
+import tf
+import actionlib
+import operator
+from time import sleep
+from math import *
+from nav_msgs.msg import Odometry
+from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal
+from actionlib_msgs.msg import GoalStatus
+
+class UMBMark:
+       def __init__(self):
+               rospy.init_node('umbmark')
+               self.move_base = actionlib.SimpleActionClient("move_base", MoveBaseAction)
+               rospy.Subscriber("odom", Odometry, self.odom_received)
+               self.odom_pose = None
+               while not self.move_base.wait_for_server(rospy.Duration(5)):
+                       rospy.loginfo("Waiting for the move_base action server to come up")
+
+       def odom_received(self, msg):
+               orientation = tf.transformations.euler_from_quaternion(msg.pose.pose.orientation.__getstate__())
+               self.odom_pose = (msg.pose.pose.position.x, msg.pose.pose.position.y, orientation[2])
+
+       def next_pos(self, x, y, angle):
+               odom_quat = tf.transformations.quaternion_from_euler(0, 0, angle)
+
+               rospy.loginfo("Moving to (%.2f, %2f), %d°.." % (x, y, angle*180/pi))
+
+               goal = MoveBaseGoal()
+               goal.target_pose.header.frame_id = "base_footprint"
+               goal.target_pose.header.stamp = rospy.Time.now()
+               goal.target_pose.pose.position.x = x
+               goal.target_pose.pose.position.y = y
+               goal.target_pose.pose.orientation.x = odom_quat[0]
+               goal.target_pose.pose.orientation.y = odom_quat[1]
+               goal.target_pose.pose.orientation.z = odom_quat[2]
+               goal.target_pose.pose.orientation.w = odom_quat[3]
+               self.move_base.send_goal(goal)
+
+               self.move_base.wait_for_result()
+
+               if self.move_base.get_state() == GoalStatus.SUCCEEDED:
+                       rospy.loginfo("The base moved to (%.2f, %2f), %d°" % (x, y, angle*180/pi))
+               else:
+                       rospy.logerr("The base failed to (%.2f, %2f), %d°" % (x, y, angle*180/pi))
+                       raise
+
+       def run(self, direction=-1):
+               while self.odom_pose is None:
+                       sleep(0.1)
+               init_pose = self.odom_pose
+               for i in range(4):
+                       self.next_pos(2, 0, 0)
+                       self.next_pos(0, 0, direction*90*pi/180)
+               final_pose = map(operator.sub, self.odom_pose, init_pose)
+               print "Odom Pose: x=%.2f, y=%.2f, angle=%.2f°" % (final_pose[0], final_pose[1], final_pose[2]*180/pi)
+
+       def run_cw(self):
+               self.run(-1)
+
+       def run_ccw(self):
+               self.run(1)
+
+if __name__ == "__main__":
+       p = UMBMark()
+       if len(sys.argv) > 1 and sys.argv[1] == "ccw":
+               p.run_ccw()
+       else:
+               p.run_cw()
index ac82b36..0f7f93e 100755 (executable)
@@ -20,8 +20,8 @@ class Waypoint:
                goal.target_pose.header.frame_id = "base_footprint"
                goal.target_pose.header.stamp = rospy.Time.now()
 
-               goal.target_pose.pose.position.x = 1.0;
-               goal.target_pose.pose.orientation.w = 1.0;
+               goal.target_pose.pose.position.x = 1.0
+               goal.target_pose.pose.orientation.w = 1.0
 
                self.move_base.send_goal(goal)