From 6925d3469d5f22b6271ece74540c0a30a51942db Mon Sep 17 00:00:00 2001 From: Erik Andresen Date: Sun, 6 Sep 2015 10:51:32 +0200 Subject: [PATCH] added umbmark.py, tested with simulation --- config/control.yaml | 4 +- config/global_costmap_params_odom.yaml | 11 +++ launch/gazebo.launch | 5 +- launch/move_base.launch | 10 ++- scripts/umbmark.py | 98 ++++++++++++++++++++++++++ scripts/waypoint.py | 4 +- 6 files changed, 124 insertions(+), 8 deletions(-) create mode 100644 config/global_costmap_params_odom.yaml create mode 100755 scripts/umbmark.py diff --git a/config/control.yaml b/config/control.yaml index 648218d..97d83a5 100644 --- a/config/control.yaml +++ b/config/control.yaml @@ -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 index 0000000..3dd3c90 --- /dev/null +++ b/config/global_costmap_params_odom.yaml @@ -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'} diff --git a/launch/gazebo.launch b/launch/gazebo.launch index b93b250..da5f559 100644 --- a/launch/gazebo.launch +++ b/launch/gazebo.launch @@ -1,7 +1,10 @@ + + + true @@ -25,7 +28,7 @@ - + diff --git a/launch/move_base.launch b/launch/move_base.launch index b6924bd..441dbea 100644 --- a/launch/move_base.launch +++ b/launch/move_base.launch @@ -2,13 +2,16 @@ + - - + + + + @@ -26,7 +29,8 @@ - + + diff --git a/scripts/umbmark.py b/scripts/umbmark.py new file mode 100755 index 0000000..d103b5c --- /dev/null +++ b/scripts/umbmark.py @@ -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() diff --git a/scripts/waypoint.py b/scripts/waypoint.py index ac82b36..0f7f93e 100755 --- a/scripts/waypoint.py +++ b/scripts/waypoint.py @@ -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) -- 2.39.2