def rotate(self, angle, tolerance):
(self.position, self.rotation) = self.get_odom()
move_cmd = Twist()
# Set the movement command to a rotation
if angle > 0:
move_cmd.angular.z = self.angular_speed
else:
move_cmd.angular.z = -1*self.angular_speed
angle = -1*angle
# Track the last angle measured
last_angle = self.rotation
# Track how far we have turned
turn_angle = 0
while abs(turn_angle + tolerance) < abs(angle) and not rospy.is_shutdown():
# Publish the Twist message and sleep 1 cycle
self.cmd_vel.publish(move_cmd)
self.r.sleep()
# Get the current rotation
(self.position, self.rotation) = self.get_odom()
# Compute the amount of rotation since the last loop
delta_angle = normalize_angle(self.rotation - last_angle)
# Add to the running total
turn_angle += delta_angle
last_angle = self.rotation
def rotate(sign):
# Initialize the movement command
move_cmd = Twist()
# Track the last angle measured
last_angle = get_odom()[1]
# Track how far we have turned
turn_angle = 0
# Turn right
move_cmd.angular.z = angular_speed * sign
while (
abs(turn_angle + angular_tolerance) < abs(goal_angle)
and not rospy.is_shutdown()
):
cmd_vel_pub.publish(move_cmd)
r.sleep()
# Get the current rotation
rotation = get_odom()[1]
# Compute the amount of rotation since the last loop
delta_angle = normalize_angle(rotation - last_angle)
# Add to the running total
turn_angle += delta_angle
last_angle = rotation
rotation = get_odom()[1]
print("Rotating robot...:", rotation)
def old_rotate(self, angle):
last_angle = self.rot
turn_angle = 0
while abs(turn_angle + self.angular_tolerance) < abs(
angle) and not rospy.is_shutdown():
self.rotate_inc()
self.update_odom()
delta_angle = normalize_angle(self.rot - last_angle)
turn_angle += delta_angle
last_angle = self.rot
self.cmd_vel.publish(Twist())
def turn(self, val):
# How fast will we update the robot's movement?
rate = 20
# Set the equivalent ROS rate variable
r = rospy.Rate(rate)
# Initialize the position variable as a Point type
position = Point()
# Loop once for each leg of the trip
#############
# Initialize the movement command
move_cmd = Twist()
# Set the movement command to forward motion
move_cmd.linear.x = self.linear_speed
# Set the movement command to a rotation
move_cmd.angular.z = self.angular_speed
# Get the starting position values
(position, rotation) = self.get_odom()
x_start = position.x
y_start = position.y
# Track the last angle measured
last_angle = rotation
# Track how far we have turned
turn_angle = 0
while abs(turn_angle + self.angular_tolerance) < abs(self.goal_angle) and not rospy.is_shutdown():
# Publish the Twist message and sleep 1 cycle
self.cmd_vel.publish(move_cmd)
r.sleep()
# Get the current rotation
(position, rotation) = self.get_odom()
# Compute the amount of rotation since the last loop
delta_angle = normalize_angle(rotation - last_angle)
# Add to the running total
turn_angle += delta_angle
last_angle = rotation
# Stop the robot for good
self.cmd_vel.publish(Twist())
return True
def execute(self, userdata):
rospy.loginfo("Executing Rotate Class...")
# Get the starting position values
(position, rotation) = self.get_odom()
last_angle = rotation
turn_angle = 0
rotate_command = Twist()
rotate_command.angular.z = self.angular_speed
#and not userdata.detectTag_Flag
while abs(turn_angle + self.angular_tolerance) < abs(
self.goal_angle) and not rospy.is_shutdown():
if self.preempt_requested():
rospy.loginfo("State Rotate is being preempted!!!")
self.service_preempt()
return 'not_full_rotate'
# Publish the Twist message and sleep 1 cycle
self.cmd_vel.publish(rotate_command)
self.r.sleep()
# Get the current rotation
(position, rotation) = self.get_odom()
# Compute the amount of rotation since the last loop
delta_angle = normalize_angle(rotation - last_angle)
# Add to the running total
turn_angle += delta_angle
last_angle = rotation
# Stop the robot before the next leg
rotate_command = Twist()
self.cmd_vel.publish(rotate_command)
rospy.sleep(1)
return 'full_rotate'
def rotate_radians(self, goal_angle):
angular_speed = 0.5
angular_tolerance = 0.02
(position, rotation) = self.get_odom()
move_cmd = Twist()
self.cmd_vel.publish(move_cmd)
rospy.sleep(1)
move_cmd.angular.z = angular_speed
if goal_angle < 0:
move_cmd.angular.z = -1 * angular_speed
# Track the last angle measured
last_angle = rotation
# Track how far we have turned
turn_angle = 0
while abs(turn_angle + angular_tolerance) < abs(
goal_angle) and not rospy.is_shutdown():
self.cmd_vel.publish(move_cmd)
self.r.sleep()
(position, rotation) = self.get_odom()
delta_angle = normalize_angle(rotation - last_angle)
turn_angle += delta_angle
last_angle = rotation
move_cmd = Twist()
self.cmd_vel.publish(move_cmd)
rospy.sleep(1)
(position, rotation) = self.get_odom()
def execute(self, userdata):
rospy.loginfo("Rotating...")
# Get the starting position values
(position, rotation) = self.get_odom()
last_angle = rotation
turn_angle = 0
rotate_command = Twist()
rotate_command.angular.z = self.angular_speed
while abs(turn_angle + self.angular_tolerance) < abs(self.goal_angle) and not rospy.is_shutdown():
if self.preempt_requested():
rospy.loginfo("State Rotate360 is being preempted!!!")
self.service_preempt()
return "not_full_rotate"
# Publish the Twist message and sleep 1 cycle
self.cmd_vel.publish(rotate_command)
self.r.sleep()
# Get the current rotation
(position, rotation) = self.get_odom()
# Compute the amount of rotation since the last loop
delta_angle = normalize_angle(rotation - last_angle)
# Add to the running total
turn_angle += delta_angle
last_angle = rotation
# Stop the robot before the next leg
rotate_command = Twist()
self.cmd_vel.publish(rotate_command)
rospy.sleep(1)
return "full_rotate"
def move_around(self, start_rotation, goal_radius=pi / 6):
self._turn = PROCESS
move_cmd = Twist()
# Set the movement command to a rotation
move_cmd.angular.z = self.angular_speed
if goal_radius < 0:
move_cmd.angular.z = -self.angular_speed
goal_radius = -goal_radius
# Track the last angle measured
last_angle = start_rotation
# Track how far we have turned
turn_angle = 0
while abs(turn_angle + self.angular_tolerance) < abs(
goal_radius) and not rospy.is_shutdown():
# Publish the Twist message and sleep 1 cycle
self.cmd_vel.publish(move_cmd)
self.r.sleep()
# Get the current rotation
(position, rotation) = self.get_odom()
# Compute the amount of rotation since the last loop
delta_angle = normalize_angle(rotation - last_angle)
# Add to the running total
turn_angle += delta_angle
last_angle = rotation
# Stop the robot
move_cmd = Twist()
self.cmd_vel.publish(move_cmd)
rospy.sleep(1)
print("Turn {} radius end!".format(goal_radius))
self._turn = FINISH
def __init__(self):
# Give the node a name
rospy.init_node('back_and_forth', anonymous=False)
# Set rospy to execute a shutdown function when exiting
rospy.on_shutdown(self.shutdown)
# Publisher to control the robot's speed
self.cmd_vel = rospy.Publisher('/cmd_vel', Twist, queue_size=5)
# How fast will we update the robot's movement?
rate = 50
# Set the equivalent ROS rate variable
r = rospy.Rate(rate)
# Set the forward linear speed to 0.15 meters per second
linear_speed = 0.2
# Set the travel distance in meters
goal_distance = 1.0
# Set the rotation speed in radians per second
angular_speed = 1
# Set the angular tolerance in degrees converted to radians
angular_tolerance = radians(1.0)
# Set the rotation angle to Pi radians (180 degrees)
goal_angle = pi/2 - ((pi/2)/20)
# Initialize the tf listener
self.tf_listener = tf.TransformListener()
# Give tf some time to fill its buffer
rospy.sleep(2)
# Set the odom frame
self.odom_frame = '/odom'
# Find out if the robot uses /base_link or /base_footprint
try:
self.tf_listener.waitForTransform(self.odom_frame, '/base_footprint', rospy.Time(), rospy.Duration(1.0))
self.base_frame = '/base_footprint'
except (tf.Exception, tf.ConnectivityException, tf.LookupException):
try:
self.tf_listener.waitForTransform(self.odom_frame, '/base_link', rospy.Time(), rospy.Duration(1.0))
self.base_frame = '/base_link'
except (tf.Exception, tf.ConnectivityException, tf.LookupException):
rospy.loginfo("Cannot find transform between /odom and /base_link or /base_footprint")
rospy.signal_shutdown("tf Exception")
# Initialize the position variable as a Point type
position = Point()
# Loop once for each leg of the trip
for i in range(4):
# Initialize the movement command
move_cmd = Twist()
# Set the movement command to forward motion
move_cmd.linear.x = linear_speed
# Get the starting position values
(position, rotation) = self.get_odom()
x_start = position.x
y_start = position.y
# Keep track of the distance traveled
distance = 0
# Enter the loop to move along a side
while distance < goal_distance and not rospy.is_shutdown():
# Publish the Twist message and sleep 1 cycle
self.cmd_vel.publish(move_cmd)
r.sleep()
# Get the current position
(position, rotation) = self.get_odom()
# Compute the Euclidean distance from the start
distance = sqrt(pow((position.x - x_start), 2) +
pow((position.y - y_start), 2))
# Stop the robot before the rotation
move_cmd = Twist()
self.cmd_vel.publish(move_cmd)
rospy.sleep(1)
# Set the movement command to a rotation
move_cmd.angular.z = angular_speed
# Track the last angle measured
last_angle = rotation
# Track how far we have turned
turn_angle = 0
while abs(turn_angle + angular_tolerance) < abs(goal_angle) and not rospy.is_shutdown():
# Publish the Twist message and sleep 1 cycle
self.cmd_vel.publish(move_cmd)
r.sleep()
# Get the current rotation
(position, rotation) = self.get_odom()
# Compute the amount of rotation since the last loop
delta_angle = normalize_angle(rotation - last_angle)
# Add to the running total
turn_angle += delta_angle
last_angle = rotation
# Stop the robot before the next leg
move_cmd = Twist()
self.cmd_vel.publish(move_cmd)
rospy.sleep(1)
# Stop the robot for good
self.cmd_vel.publish(Twist())
def __init__(self):
# Give the node a name
rospy.init_node('nav_square', anonymous=False)
# Set rospy to execute a shutdown function when terminating the script
rospy.on_shutdown(self.shutdown)
# How fast will we check the odometry values?
rate = 20
# Set the equivalent ROS rate variable
r = rospy.Rate(rate)
# Set the parameters for the target square
goal_distance = rospy.get_param("~goal_distance", 1.0) # meters
goal_angle = rospy.get_param("~goal_angle", radians(90)) # degrees converted to radians
linear_speed = rospy.get_param("~linear_speed", 0.2) # meters per second
angular_speed = rospy.get_param("~angular_speed", 0.7) # radians per second
angular_tolerance = rospy.get_param("~angular_tolerance", radians(2)) # degrees to radians
# Publisher to control the robot's speed
self.cmd_vel = rospy.Publisher('/cmd_vel', Twist, queue_size=5)
# The base frame is base_footprint for the TurtleBot but base_link for Pi Robot
self.base_frame = rospy.get_param('~base_frame', '/base_link')
# The odom frame is usually just /odom
self.odom_frame = rospy.get_param('~odom_frame', '/odom')
# Initialize the tf listener
self.tf_listener = tf.TransformListener()
# Give tf some time to fill its buffer
rospy.sleep(2)
# Set the odom frame
self.odom_frame = '/odom'
# Find out if the robot uses /base_link or /base_footprint
try:
self.tf_listener.waitForTransform(self.odom_frame, '/base_footprint', rospy.Time(), rospy.Duration(1.0))
self.base_frame = '/base_footprint'
except (tf.Exception, tf.ConnectivityException, tf.LookupException):
try:
self.tf_listener.waitForTransform(self.odom_frame, '/base_link', rospy.Time(), rospy.Duration(1.0))
self.base_frame = '/base_link'
except (tf.Exception, tf.ConnectivityException, tf.LookupException):
rospy.loginfo("Cannot find transform between /odom and /base_link or /base_footprint")
rospy.signal_shutdown("tf Exception")
# Initialize the position variable as a Point type
position = Point()
# Cycle through the four sides of the square
for i in range(4):
# Initialize the movement command
move_cmd = Twist()
# Set the movement command to forward motion
move_cmd.linear.x = linear_speed
# Get the starting position values
(position, rotation) = self.get_odom()
x_start = position.x
y_start = position.y
# Keep track of the distance traveled
distance = 0
# Enter the loop to move along a side
while distance < goal_distance and not rospy.is_shutdown():
# Publish the Twist message and sleep 1 cycle
self.cmd_vel.publish(move_cmd)
r.sleep()
# Get the current position
(position, rotation) = self.get_odom()
# Compute the Euclidean distance from the start
distance = sqrt(pow((position.x - x_start), 2) +
pow((position.y - y_start), 2))
# Stop the robot before rotating
move_cmd = Twist()
self.cmd_vel.publish(move_cmd)
rospy.sleep(1.0)
# Set the movement command to a rotation
move_cmd.angular.z = angular_speed
# Track the last angle measured
last_angle = rotation
# Track how far we have turned
turn_angle = 0
# Begin the rotation
while abs(turn_angle + angular_tolerance) < abs(goal_angle) and not rospy.is_shutdown():
# Publish the Twist message and sleep 1 cycle
self.cmd_vel.publish(move_cmd)
r.sleep()
# Get the current rotation
(position, rotation) = self.get_odom()
# Compute the amount of rotation since the last lopp
delta_angle = normalize_angle(rotation - last_angle)
turn_angle += delta_angle
last_angle = rotation
move_cmd = Twist()
self.cmd_vel.publish(move_cmd)
rospy.sleep(1.0)
# Stop the robot when we are done
self.cmd_vel.publish(Twist())
def __init__(self):
# Give the node a name
rospy.init_node('initial_odom_circle', anonymous=False)
# Set rospy to execute a shutdown function when exiting
rospy.on_shutdown(self.shutdown)
# Publisher to control the robot's speed
self.cmd_vel = rospy.Publisher('/cmd_vel_mux/input/navi', Twist, queue_size = 10)
# How fast will we update the robot's movement?
rate = 20
# Set the equivalent ROS rate variable
r = rospy.Rate(rate)
# Set the forward linear speed to 0.2 meters per second
linear_speed = 0.0
# Set the travel distance in meters
goal_distance = 0.0
# Set the rotation speed in radians per second
angular_speed = 0.3
# Set the angular tolerance in degrees converted to radians
angular_tolerance = radians(2.5)
# Set the rotation angle to Pi radians (180 degrees)
goal_angle = 2*pi
# Initialize the tf listener
self.tf_listener = tf.TransformListener()
# Give tf some time to fill its buffer
rospy.sleep(2)
# Set the odom frame
self.odom_frame = '/odom'
# Find out if the robot uses /base_link or /base_footprint
try:
self.tf_listener.waitForTransform(self.odom_frame, '/base_footprint', rospy.Time(), rospy.Duration(1.0))
self.base_frame = '/base_footprint'
except (tf.Exception, tf.ConnectivityException, tf.LookupException):
try:
self.tf_listener.waitForTransform(self.odom_frame, '/base_link', rospy.Time(), rospy.Duration(1.0))
self.base_frame = '/base_link'
except (tf.Exception, tf.ConnectivityException, tf.LookupException):
rospy.loginfo("Cannot find transform between /odom and /base_link or /base_footprint")
rospy.signal_shutdown("tf Exception")
# Initialize the position variable as a Point type
position = Point()
# Loop once for each leg of the trip
# Initialize the movement command
move_cmd = Twist()
# Set the movement command to forward motion
move_cmd.linear.x = linear_speed
# Set the movement command to a rotation
move_cmd.angular.z = angular_speed
# Get the starting position values
(position, rotation) = self.get_odom()
x_start = position.x
y_start = position.y
# Track the last angle measured
last_angle = rotation
# Track how far we have turned
turn_angle = 0
while abs(turn_angle + angular_tolerance) < abs(goal_angle) and not rospy.is_shutdown():
# Publish the Twist message and sleep 1 cycle
self.cmd_vel.publish(move_cmd)
r.sleep()
# Get the current rotation
(position, rotation) = self.get_odom()
# Compute the amount of rotation since the last loop
delta_angle = normalize_angle(rotation - last_angle)
# Add to the running total
turn_angle += delta_angle
last_angle = rotation
# Stop the robot for good
self.cmd_vel.publish(Twist())
def __init__(self):
# Creat a node named out_and_back
rospy.init_node('nav_square', anonymous=False)
# Set rospy to execute a shutdown function when exiting
rospy.on_shutdown(self.shutdown)
# variable definition
rate = 20 # moving friquency ,How fast will we update the robot's movement?
r = rospy.Rate(
rate) # friquency variable, Set the equivalent ROS rate variable
# Set the parameters for the target square ~ private paramters
goal_distance = rospy.get_param("~goal_distance", 1.0) # meters
goal_angle = radians(rospy.get_param(
"~goal_angle", 90)) # degrees converted to radians
linear_speed = rospy.get_param("~linear_speed",
0.2) # meters per second
angular_speed = rospy.get_param("~angular_speed",
0.7) # radians per second
angular_tolerance = radians(rospy.get_param("~angular_tolerance",
1)) # degrees to radians
# Creat a Publisher, to control the robot's speed topic message_type buffer_size
self.cmd_vel = rospy.Publisher('/cmd_vel', Twist, queue_size=5)
# The base frame is base_footprint for the TurtleBot but base_link for Pi Robot
self.base_frame = rospy.get_param('~base_frame', '/base_link')
# The odom frame is usually just /odom
self.odom_frame = rospy.get_param('~odom_frame', '/odom')
# Initialize the tf listener
self.tf_listener = tf.TransformListener()
# Give tf some time to fill its buffer
rospy.sleep(2)
# Set the odom frame
self.odom_frame = '/odom' # world-fixed frame
# Find the reference frame
# Find out if the robot uses /base_link or /base_footprint
# /base_footprint frame used by the TurtleBot
# /base_link frame used by Pi Robot and Maxwell
try:
self.tf_listener.waitForTransform(
self.odom_frame, '/base_footprint', rospy.Time(),
rospy.Duration(1.0)) #wait for tramsrform
self.base_frame = '/base_footprint' # the robot uses /base_footprint frame
except (tf.Exception, tf.ConnectivityException,
tf.LookupException): # Exception = error state
try:
self.tf_listener.waitForTransform(self.odom_frame,
'/base_link', rospy.Time(),
rospy.Duration(1.0))
self.base_frame = '/base_link'
except (tf.Exception, tf.ConnectivityException,
tf.LookupException):
rospy.loginfo(
"Cannot find transform between /odom and /base_link or /base_footprint"
)
rospy.signal_shutdown("tf Exception")
# Initialize the position variable as a Point type(comes from geometry_msgs.msg which has x,y,z)
position = Point()
# Cycle through the four sides of the square
for i in range(4):
# Straight forward
# initialize the movement command with linear velocity
move_cmd = Twist(
) # Initialize the movement command to zero , Twist:linear.x linear.y linear.z Point: x,y,z
move_cmd.linear.x = linear_speed # Set the movement command to forward motion with desired speed
#record the starting position
(position,
rotation) = self.get_odom() # Get the starting position values
x_start = position.x
y_start = position.y
distance = 0 # traveled distance
# travel with a linear speed
while distance < goal_distance and not rospy.is_shutdown(
): # hasno't reached the desired distance
self.cmd_vel.publish(
move_cmd
) # Publish the Twist message velocity comannd until reached the destination
r.sleep()
(position,
rotation) = self.get_odom() # Get the current position
# Compute the Euclidean distance from the start
distance = sqrt(
pow((position.x -
x_start), 2) + # update the traveled distance
pow((position.y - y_start), 2))
# Stop the robot before rotating
# stop()
move_cmd = Twist()
self.cmd_vel.publish(move_cmd)
rospy.sleep(1.0)
# rotate
move_cmd.angular.z = angular_speed # Set the movement command to a rotation
last_angle = rotation # starting angle, odometry record the last angle measured(starting angle)
turn_angle = 0 # traveled angle
while abs(turn_angle + angular_tolerance) < abs(
goal_angle) and not rospy.is_shutdown(
): # hasno't reached the desired goal_angle
self.cmd_vel.publish(
move_cmd
) # Publish the Twist message velocity comannd angular_speed until reached the desired goal_angle
r.sleep() # sleep with rate
(position,
rotation) = self.get_odom() # Get the current rotation angle
delta_angle = normalize_angle(
rotation - last_angle
) # compute the the delt_angel constraint with in -pi to pi
turn_angle += delta_angle # update the traveled turn_angle
last_angle = rotation # update the last_angle
# Stop the robot before the next leg
# stop()
move_cmd = Twist()
self.cmd_vel.publish(move_cmd)
rospy.sleep(1.0)
# Stop the robot when we are done
self.cmd_vel.publish(Twist())
def __init__(self):
# Give the node a name
rospy.init_node('move_robot', anonymous=False)
# Set rospy to execute a shutdown function when exiting
rospy.on_shutdown(self.shutdown)
# Publisher to control the robot's speed
self.cmd_vel = rospy.Publisher('/cmd_vel', Twist, queue_size=5)
#self.cmd_vel = rospy.Publisher('/mobile_base/commands/velocity', Twist, queue_size=5)
# How fast will we update the robot's movement?
rate = 20
# Set the equivalent ROS rate variable
r = rospy.Rate(rate)
# Set the forward linear speed to 0.15 meters per second
linear_speed = 0.15
# Set the rotation speed in radians per second
angular_speed = 0.5
# Set the angular tolerance in degrees converted to radians
angular_tolerance = radians(1.0)
# Initialize the tf listener
self.tf_listener = tf.TransformListener()
# Give tf some time to fill its buffer
#rospy.sleep(2)
# Set the odom frame
self.odom_frame = '/odom'
# Find out if the robot uses /base_link or /base_footprint
try:
self.tf_listener.waitForTransform(self.odom_frame, '/base_footprint', rospy.Time(), rospy.Duration(1.0))
self.base_frame = '/base_footprint'
except (tf.Exception, tf.ConnectivityException, tf.LookupException):
try:
self.tf_listener.waitForTransform(self.odom_frame, '/base_link', rospy.Time(), rospy.Duration(1.0))
self.base_frame = '/base_link'
except (tf.Exception, tf.ConnectivityException, tf.LookupException):
rospy.loginfo("Cannot find transform between /odom and /base_link or /base_footprint")
rospy.signal_shutdown("tf Exception")
# Initialize the position variable as a Point type
position = Point()
# Following points in path
for i in range(1, len(path)):
# Set parameters for current target
# Initialize the movement command
move_cmd = Twist()
# Get the starting position values
(position, rotation) = self.get_odom()
x_start = position.x
y_start = position.y
# Scale the points in path.txt by 0.01
goal = Point(path[i][0]*0.01, path[i][1]*0.01, 0)
goal_distance = sqrt((goal.x-x_start)**2 + (goal.y-y_start)**2)
goal_angle = np.arctan2(goal.y-y_start, goal.x-x_start)
goal_angle -= rotation
if goal_angle < 0:
goal_angle += 2*pi
# Rotate before moving
print("Moving towards ("+str(goal.x)+", "+str(goal.y)+")!")
# Set the movement command to a rotation
move_cmd.angular.z = angular_speed
# Track the last angle measured
last_angle = rotation
# Track how far we have turned
turn_angle = 0
while abs(turn_angle + angular_tolerance) < abs(goal_angle) and not rospy.is_shutdown():
# Publish the Twist message and sleep 1 cycle
self.cmd_vel.publish(move_cmd)
r.sleep()
# Get the current rotation
(position, rotation) = self.get_odom()
# Compute the amount of rotation since the last loop
delta_angle = normalize_angle(rotation - last_angle)
# Add to the running total
turn_angle += delta_angle
last_angle = rotation
# Stop the robot before moving
move_cmd = Twist()
self.cmd_vel.publish(move_cmd)
rospy.sleep(1)
# Set the movement command to forward motion
move_cmd.linear.x = linear_speed
# Keep track of the distance traveled
distance = 0
# Enter the loop to move along a side
while distance < goal_distance and not rospy.is_shutdown():
print("Moving towards ("+str(goal.x)+", "+str(goal.y)+")!")
# Publish the Twist message and sleep 1 cycle
self.cmd_vel.publish(move_cmd)
r.sleep()
# Get the current position
(position, rotation) = self.get_odom()
# Compute the Euclidean distance from the start
distance = sqrt(pow((position.x - x_start), 2) +
pow((position.y - y_start), 2))
print("Current position: (" + str(position.x) + ", " + str(position.y) + "), " + str(distance) + " away from position ("+str(goal.x)+", "+str(goal.y)+")!")
print("Robot reaches ("+str(goal.x)+", "+str(goal.y)+")!")
# Stop the robot before the next edge
move_cmd = Twist()
self.cmd_vel.publish(move_cmd)
rospy.sleep(1)
print("Robot reaches final goal!")
# Stop the robot for good
self.cmd_vel.publish(Twist())
def __init__(self):
rospy.init_node('out_and_back', anonymous=True)
rospy.on_shutdown(self.shutdown)
self.cmd_vel = rospy.Publisher('/cmd_vel', Twist)
rate = 20
r = rospy.Rate(rate)
#linear_speed=0.2
linear_speed = 0.15
goal_distance = 1.0
#angular_speed=1.0
angular_speed = 0.15
angular_tolerance = radians(2.5)
goal_angle = pi
self.tf_listener = tf.TransformListener()
rospy.sleep(2)
self.odom_frame = '/odom'
try:
self.tf_listener.waitForTransform(self.odom_frame,
'/base_footprint', rospy.Time(),
rospy.Duration(1, 0))
self.base_frame = '/base_footprint'
print(self.base_frame)
except (tf.Exception, tf.ConnectivityException, tf.LookupException):
try:
self.tf_listener.waitForTransform(self.odom_frame,
'/base_link', rospy.Time(),
rospy.Duration(1.0))
self.base_frame = '/base_link'
except (tf.Exception, tf.ConnectivityException,
tf.LookupException):
rospy.loginfo(
'Cannot find transform between /odom and /base_link or /base_footprint'
)
rospy.signal_shutdown('tf Exception')
position = Point()
for i in range(2):
move_cmd = Twist()
move_cmd.linear.x = linear_speed
(position, rotation) = self.get_odom()
x_start = position.x
y_start = position.y
distance = 0
while distance < goal_distance and not rospy.is_shutdown():
self.cmd_vel.publish(move_cmd)
r.sleep()
(position, rotation) = self.get_odom()
distance = sqrt(
pow((position.x - x_start), 2) +
pow((position.x - y_start), 2))
move_cmd = Twist()
self.cmd_vel.publish(move_cmd)
rospy.sleep(1)
move_cmd.angular.z = angular_speed
last_angle = rotation
turn_angle = 0
while abs(turn_angle + angular_tolerance) < abs(
goal_angle) and not rospy.is_shutdown():
self.cmd_vel.publish(move_cmd)
r.sleep()
(position, rotation) = self.get_odom()
delta_angle = normalize_angle(rotation - last_angle)
turn_angle += delta_angle
last_angle = rotation
move_cmd = Twist()
self.cmd_vel.publish(move_cmd)
rospy.sleep(1)
self.cmd_vel.publish(Twist())
def __init__(self):
# Initializes a Node
rospy.init_node('odom_out_and_back', anonymous=False)
rospy.on_shutdown(self.shutdown)
rospy.loginfo("Robot Started")
# Initializes a topic to publish velocity of the robot
self.cmd_vel = rospy.Publisher('/cmd_vel', Twist)
# Rate at which the velocity would be published
rate = 20
r = rospy.Rate(rate)
# Intended Linear Speed of the Robot
linear_speed = 0.2
# Distance of the Goal from the Start Poisition
goal_distance = 5.0
# Intended Angular Speed of the Robot
angular_speed = 1.0
# Angle of the Goal
goal_angle = pi
angular_tolerance = radians(2.5)
self.tflistener = tf.TransformListener()
rospy.sleep(2)
self.odom_frame = '/odom'
# Check whether transform is on /base_link frame or /base_footprint frame
try:
self.tflistener.waitForTransform(self.odom_frame,
'/base_footprint', rospy.Time(),
rospy.Duration(1.0))
self.base_frame = '/base_footprint'
except (tf.Exception, tf.ConnectivityException, tf.LookupException):
try:
self.tflistener.waitForTransform(self.odom_frame, '/base_link',
rospy.Time(),
rospy.Duration(1.0))
self.base_frame = '/base_link'
except (tf.Exception, tf.ConnectivityException,
tf.LookupException):
rospy.loginfo("No Base Frame Found")
rospy.signal_shutdown("tf exception")
position = Point()
# Loop to move the robot forward and then back again
for i in range(2):
move_cmd = Twist()
move_cmd.linear.x = linear_speed
(position, rotation) = self.get_odom()
rospy.loginfo("here")
x_start = position.x
y_start = position.y
distance = 0
# Check whether the robot reached its goal
while (distance < goal_distance) and not rospy.is_shutdown():
self.cmd_vel.publish(move_cmd)
r.sleep()
(position, rotation) = self.get_odom()
distance = sqrt(
pow((position.x - x_start), 2) +
pow((position.y - y_start), 2))
rospy.loginfo("Complete 1st move")
move_cmd = Twist()
self.cmd_vel.publish(move_cmd)
rospy.sleep(1)
move_cmd.angular.z = angular_speed
last_angle = rotation
turn_angle = 0
# Check whether the robot has turned as much as the goal angle
while (abs(turn_angle + angular_tolerance) < abs(goal_angle)
and not rospy.is_shutdown()):
self.cmd_vel.publish(move_cmd)
r.sleep()
(position, rotation) = self.get_odom()
delta_angle = normalize_angle(rotation - last_angle)
turn_angle += delta_angle
last_angle = rotation
move_cmd = Twist()
self.cmd_vel.publish(move_cmd)
rospy.sleep(1)
self.cmd_vel.publish(Twist())
rospy.sleep(1)
def __init__(self):
rospy.init_node("out_and_back",anonymous = False)
rospy.on_shutdown(self.shutdown)
self.cmd_vel = rospy.Publisher("/cmd_vel",Twist,queue_size = 5)
rate = 20
r = rospy.Rate(rate)
linear_speed = 0.2
goal_distance = 1.0
angular_speed = 1.0
goal_tolerance = radians(2.5)
goal_angle = pi
self.tf_listener = tf.TransformListener()
rospy.sleep(2)
self.odom_frame = "/odom"
try:
self.tf_listener.waitForTransform(self.odom_frame,'/base_footprint',rospy.Time(),rospy.Duration(1.0))
self.base_frame = "/base_footprint"
except (tf.Exception,tf.ConnectivityException,tf.LookupException):
try:
self.tf_listener.waitFotTransform(self.odom_frame,"/base_link",rospy.Time(),rospy.Duration(1.0))
self.base_frame = "/base_link"
except (tf.Exception,tf.ConnectivityException,tf.LoopupException):
rospy.loginfo("can not find transform between /odom and /base_link and /base_footprint")
rospy.signal_shutdown("tf Exception")
position = Point()
for i in range(2):
move_cmd = Twist()
move_cmd.linear.x = linear_speed
(position,rotation) = self.get_odom(0)
x_start = position.x
y_start = position.y
distance = 0
while distance < goal_distance and not rospy.is_shutdown():
self.cmd_vel.publish(move_cmd)
r.sleep()
(position,rotation) = self.get_odom()
distance = sqrt(pow((position.x-x_start),2)+pow((position.y-y_start),2))
move_cmd = Twist()
self.cmd_vel.publish(move_cmd)
rospy.sleep(1)
move_cmd.angular.z = angular_speed
last_angle = rotation
turn_angle = 0
while abs(turn_angle + angle_tolerance) < abs(angular_goal) and not rospuy.is_shutdown():
self.cmd_vel.publish(move_cmd)
r.sleep()
(position,rotation) = self.get_odom()
delta_angle = normalize_angle(rotation - last_angle)
turn_angle += delta_angle
last_angle = rotation
move_cmd = Twist()
self.cmd_vel.publish(move_cmd)
rospy.sleep(1)
self.cmd_vel.publish(Twist())
def __init__(self):
# Give the node a name
rospy.init_node('calibrate_angular', anonymous=False)
# Set rospy to execute a shutdown function when terminating the script
rospy.on_shutdown(self.shutdown)
# How fast will we check the odometry values?
self.rate = rospy.get_param('~rate', 20)
r = rospy.Rate(self.rate)
# The test angle is 360 degrees
self.test_angle = radians(rospy.get_param('~test_angle', 360.0))
self.speed = rospy.get_param('~speed', 0.5) # radians per second
self.tolerance = radians(rospy.get_param(
'tolerance', 1)) # degrees converted to radians
self.odom_angular_scale_correction = rospy.get_param(
'~odom_angular_scale_correction', 1.0)
self.start_test = rospy.get_param('~start_test', True)
# Publisher to control the robot's speed
self.cmd_vel = rospy.Publisher('/cmd_vel', Twist, queue_size=5)
# Fire up the dynamic_reconfigure server
dyn_server = Server(CalibrateAngularConfig,
self.dynamic_reconfigure_callback)
# Connect to the dynamic_reconfigure server
dyn_client = dynamic_reconfigure.client.Client("calibrate_angular",
timeout=60)
# The base frame is usually base_link or base_footprint
self.base_frame = rospy.get_param('~base_frame', '/base_link')
# The odom frame is usually just /odom
self.odom_frame = rospy.get_param('~odom_frame', '/odom')
# Initialize the tf listener
self.tf_listener = tf.TransformListener()
# Give tf some time to fill its buffer
rospy.sleep(2)
# Make sure we see the odom and base frames
self.tf_listener.waitForTransform(self.odom_frame, self.base_frame,
rospy.Time(), rospy.Duration(60.0))
rospy.loginfo("Bring up rqt_reconfigure to control the test.")
reverse = 1
while not rospy.is_shutdown():
if self.start_test:
# Get the current rotation angle from tf
self.odom_angle = self.get_odom_angle()
last_angle = self.odom_angle
turn_angle = 0
self.test_angle *= reverse
error = self.test_angle - turn_angle
# Alternate directions between tests
reverse = -reverse
while abs(error) > self.tolerance and self.start_test:
if rospy.is_shutdown():
return
# Rotate the robot to reduce the error
move_cmd = Twist()
move_cmd.angular.z = copysign(self.speed, error)
self.cmd_vel.publish(move_cmd)
r.sleep()
# Get the current rotation angle from tf
self.odom_angle = self.get_odom_angle()
# Compute how far we have gone since the last measurement
delta_angle = self.odom_angular_scale_correction * normalize_angle(
self.odom_angle - last_angle)
# Add to our total angle so far
turn_angle += delta_angle
error = self.test_angle - turn_angle
last_angle = self.odom_angle
# Stop the robot
self.cmd_vel.publish(Twist())
# Update the status flag
self.start_test = False
params = {'start_test': False}
dyn_client.update_configuration(params)
rospy.sleep(0.5)
# Stop the robot
self.cmd_vel.publish(Twist())
def __init__(self):
# 给出节点的名字
rospy.init_node('out_and_back', anonymous=False)
# 设置rospy在程序退出时执行的关机函数
rospy.on_shutdown(self.shutdown)
# Publisher（发布器）：发布机器人运动的速度
self.cmd_vel = rospy.Publisher('/cmd_vel', Twist, queue_size=5)
# 我们将用多快的速度更新控制机器人运动的命令?
rate = 20
# 设定相同的值给rospy.Rate()
r = rospy.Rate(rate)
# 设定前进的线速度为0.2m/s
linear_speed = 0.2
# 设定行程距离为1.0m
goal_distance = 1.0
# 设定转动速度1.0rad/s .即大约6秒转一圈
angular_speed = 1.0
# Set the angular tolerance in degrees converted to radians
angular_tolerance = radians(2.5)
# 设定转动弧度： Pi 弧度 (180 度)
goal_angle = pi
# 初始化这个tf监听器
self.tf_listener = tf.TransformListener()
# 给tf一些时间让它填补它的缓冲区
rospy.sleep(2)
# 设置odom坐标系
self.odom_frame = '/odom'
# 询问机器人使用的是/base_link坐标系还是/base_footprint坐标系
try:
self.tf_listener.waitForTransform(self.odom_frame,
'/base_footprint', rospy.Time(),
rospy.Duration(1.0))
self.base_frame = '/base_footprint'
except (tf.Exception, tf.ConnectivityException, tf.LookupException):
try:
self.tf_listener.waitForTransform(self.odom_frame,
'/base_link', rospy.Time(),
rospy.Duration(1.0))
self.base_frame = '/base_link'
except (tf.Exception, tf.ConnectivityException,
tf.LookupException):
rospy.loginfo(
"Cannot find transform between /odom and /base_link or /base_footprint"
)
rospy.signal_shutdown("tf Exception")
# 初始化了一个Point类型的变量
position = Point()
# 往返2次行程
for i in range(2):
# 初始化运动命令
move_cmd = Twist()
# 设定前进速度
move_cmd.linear.x = linear_speed
# 得到开始的姿态信息（位置和转角）
(position, rotation) = self.get_odom()
x_start = position.x
y_start = position.y
# 随时掌控机器人行驶的距离
distance = 0
# 进入循环，沿着一边移动
while distance < goal_distance and not rospy.is_shutdown():
# 发布一次Twist消息 和 sleep 1秒
self.cmd_vel.publish(move_cmd)
r.sleep()
# 给出正确的姿态信息（位置和转角）
(position, rotation) = self.get_odom()
# 计算相对于开始位置的欧几里得距离（即位移）
distance = sqrt(
pow((position.x - x_start), 2) +
pow((position.y - y_start), 2))
# 在转动机器人前，停止它
move_cmd = Twist()
self.cmd_vel.publish(move_cmd)
rospy.sleep(1)
# 给旋转配置运动命令
move_cmd.angular.z = angular_speed
# 跟踪记录最后的角度
last_angle = rotation
# 跟踪我们已经转动了多少角度
turn_angle = 0
while abs(turn_angle + angular_tolerance) < abs(
goal_angle) and not rospy.is_shutdown():
# 发布一次Twist消息 和 sleep 1秒
self.cmd_vel.publish(move_cmd)
r.sleep()
# 给出正确的姿态信息（位置和转角）
(position, rotation) = self.get_odom()
# 计算自每次的旋转量
delta_angle = normalize_angle(rotation - last_angle)
# 添加到正在运行的合计里
turn_angle += delta_angle
last_angle = rotation
# 下一航段之前停止机器人
move_cmd = Twist()
self.cmd_vel.publish(move_cmd)
rospy.sleep(1)
# 为了机器人好，停止它
self.cmd_vel.publish(Twist())
def __init__(self):
# Give the node a name
rospy.init_node("out_and_back", anonymous=False)
# Set rospy to execute a shutdown function when exiting
rospy.on_shutdown(self.shutdown)
# Publisher to control the robot's speed
# self.cmd_vel = rospy.Publisher('/cmd_vel', Twist) #for simulator
self.cmd_vel = rospy.Publisher("/mobile_base/commands/velocity", Twist) # FOR TURTLE BOT
# How fast will we update the robot's movement?
rate = 20
# Set the equivalent ROS rate variable
r = rospy.Rate(rate)
# Set the forward linear speed to 0.2 meters per second
linear_speed = 0.2
# Set the travel distance in meters
goal_distance = 3.0
# Set the rotation speed in radians per second
angular_speed = 1.0
# Set the angular tolerance in degrees converted to radians
angular_tolerance = radians(2.5)
# Set the rotation angle to Pi radians (180 degrees)
goal_angle = pi
# Initialize the tf listener
self.tf_listener = tf.TransformListener()
# Give tf some time to fill its buffer
rospy.sleep(2)
# Set the odom frame
self.odom_frame = "/odom"
# Find out if the robot uses /base_link or /base_footprint
try:
self.tf_listener.waitForTransform(self.odom_frame, "/base_footprint", rospy.Time(), rospy.Duration(1.0))
self.base_frame = "/base_footprint"
except (tf.Exception, tf.ConnectivityException, tf.LookupException):
try:
self.tf_listener.waitForTransform(self.odom_frame, "/base_link", rospy.Time(), rospy.Duration(1.0))
self.base_frame = "/base_link"
except (tf.Exception, tf.ConnectivityException, tf.LookupException):
rospy.loginfo("Cannot find transform between /odom and /base_link or /base_footprint")
rospy.signal_shutdown("tf Exception")
# Initialize the position variable as a Point type
position = Point()
# Loop once for each leg of the trip
for i in range(2):
# Initialize the movement command
move_cmd = Twist()
# Set the movement command to forward motion
move_cmd.linear.x = linear_speed
# Get the starting position values
(position, rotation) = self.get_odom()
x_start = position.x
y_start = position.y
# Keep track of the distance traveled
distance = 0
# Enter the loop to move along a side
while distance < goal_distance and not rospy.is_shutdown():
# Publish the Twist message and sleep 1 cycle
self.cmd_vel.publish(move_cmd)
r.sleep()
# Get the current position
(position, rotation) = self.get_odom()
# Compute the Euclidean distance from the start
distance = sqrt(pow((position.x - x_start), 2) + pow((position.y - y_start), 2))
# Stop the robot before the rotation
move_cmd = Twist()
self.cmd_vel.publish(move_cmd)
rospy.sleep(1)
# Set the movement command to a rotation
move_cmd.angular.z = angular_speed
# Track the last angle measured
last_angle = rotation
# Track how far we have turned
turn_angle = 0
while abs(turn_angle + angular_tolerance) < abs(goal_angle) and not rospy.is_shutdown():
# Publish the Twist message and sleep 1 cycle
self.cmd_vel.publish(move_cmd)
r.sleep()
# Get the current rotation
(position, rotation) = self.get_odom()
# Compute the amount of rotation since the last loop
delta_angle = normalize_angle(rotation - last_angle)
# Add to the running total
turn_angle += delta_angle
last_angle = rotation
# Stop the robot before the next leg
move_cmd = Twist()
self.cmd_vel.publish(move_cmd)
rospy.sleep(1)
# Stop the robot for good
self.cmd_vel.publish(Twist())
def __init__(self):
rospy.init_node('calibration', anonymous=False)
rospy.on_shutdown(self.shutdown)
# Publisher to control robot's speed
self.cmd_vel = rospy.Publisher('/cmd_vel', Twist, queue_size=5)
# Robot movement update speed
rate = 20
r = rospy.Rate(rate)
# Forward linear speed (m/s)
linear_speed = 0.1
goal_distance = 1.0
# Rotation speed (rads/s)
angular_speed = 1.0
angular_tolerance = radians(2.5)
# Rotation angle (360 degrees)
goal_angle = 2*pi
self.tf_listener = tf.TransformListener()
rospy.sleep(2)
self.odom_frame = '/odom'
# Look if robot uses /base_link or /base_footprint
try:
self.tf_listener.waitForTransform(self.odom_frame, '/base_footprint', rospy.Time(), rospy.Duration(1.0))
self.base_frame = '/base_footprint'
except (tf.Exception, tf.ConnectivityException, tf.LookupException):
try:
self.tf_listener.waitForTransform(self.odom_frame, '/base_link', rospy.Time(), rospy.Duration(1.0))
self.base_frame = '/base_link'
except (tf.Exception, tf.ConnectivityException, tf.LookupException):
rospy.loginfo("Cannot find transform between /odom and /base_link or /base_footprint")
rospy.signal_shutdown("tf Exception")
# Initialize position variable as a Point type
position = Point()
# Loop once for each leg of the trip
for i in range(2):
# Initialize the movement command
move_cmd = Twist()
# Set the movement command to forward motion
move_cmd.linear.x = linear_speed
# Get initial position
(position, rotation) = self.get_odom()
x_start = position.x
y_start = position.y
# Keep track of the distance traveled
# distance = 0
# # Enter the loop to move along a side
# while distance < goal_distance and not rospy.is_shutdown():
# # Publish the Twist message and sleep 1 cycle
# self.cmd_vel.publish(move_cmd)
# r.sleep()
# # Get the current position
# (position, rotation) = self.get_odom()
# # Compute the Euclidean distance from the start
# distance = sqrt(pow((position.x - x_start), 2) +
# pow((position.y - y_start), 2))
### ROTATION
# Stop the robot before the rotation
move_cmd = Twist()
self.cmd_vel.publish(move_cmd)
rospy.sleep(1)
# Set the movement command to a rotation
move_cmd.angular.z = angular_speed
# Track the last angle measured
last_angle = rotation
# Track how far we have turned
turn_angle = 0
while abs(turn_angle + angular_tolerance) < abs(goal_angle) and not rospy.is_shutdown():
# Publish the Twist message and sleep 1 cycle
self.cmd_vel.publish(move_cmd)
r.sleep()
# Get current rotation
(position, rotation) = self.get_odom()
delta_angle = normalize_angle(rotation - last_angle)
turn_angle += delta_angle
last_angle = rotation
# Stop the robot before the next leg
move_cmd = Twist()
self.cmd_vel.publish(move_cmd)
rospy.sleep(1)
# Stop the robot
self.cmd_vel.publish(Twist())
def __init__(self):
# Give the node a name
rospy.init_node('calibrate_angular', anonymous=False)
# Set rospy to execute a shutdown function when terminating the script
rospy.on_shutdown(self.shutdown)
# How fast will we check the odometry values?
self.rate = 30
r = rospy.Rate(self.rate)
# The test angle is 360 degrees
self.test_angle = radians(rospy.get_param('~test_angle', 360.0))
self.speed = rospy.get_param('~speed', 0.7) # radians per second
self.tolerance = rospy.get_param('tolerance', radians(5)) # degrees converted to radians
self.odom_angular_scale_correction = rospy.get_param('~odom_angular_scale_correction', 1.0)
self.start_test = rospy.get_param('~start_test', True)
# Publisher to control the robot's speed
self.cmd_vel = rospy.Publisher('/cmd_vel_mux/input/navi', Twist, queue_size=10)
# Fire up the dynamic_reconfigure server
dyn_server = Server(CalibrateAngularConfig, self.dynamic_reconfigure_callback)
# Connect to the dynamic_reconfigure server
dyn_client = dynamic_reconfigure.client.Client("calibrate_angular", timeout=60)
# The base frame is base_footprint for the TurtleBot but base_link for Pi Robot
self.base_frame = rospy.get_param('~base_frame', '/base_link')
# The odom frame is usually just /odom
self.odom_frame = rospy.get_param('~odom_frame', '/odom')
# Initialize the tf listener
self.tf_listener = tf.TransformListener()
# Give tf some time to fill its buffer
rospy.sleep(2)
# Make sure we see the odom and base frames
self.tf_listener.waitForTransform(self.odom_frame, self.base_frame, rospy.Time(), rospy.Duration(60.0))
rospy.loginfo("Bring up rqt_reconfigure to control the test.")
reverse = 1
while not rospy.is_shutdown():
# Execute the rotation
if self.start_test:
# Get the current rotation angle from tf
self.odom_angle = self.get_odom_angle()
last_angle = self.odom_angle
turn_angle = 0
# Alternate directions between tests
reverse = -reverse
angular_speed = reverse * self.speed
while abs(turn_angle) < abs(self.test_angle):
if rospy.is_shutdown():
return
move_cmd = Twist()
move_cmd.angular.z = angular_speed
self.cmd_vel.publish(move_cmd)
r.sleep()
# Get the current rotation angle from tf
self.odom_angle = self.get_odom_angle()
# Compute how far we have gone since the last measurement
delta_angle = self.odom_angular_scale_correction * normalize_angle(self.odom_angle - last_angle)
# Add to our total angle so far
turn_angle += delta_angle
last_angle = self.odom_angle
# Stop the robot
self.cmd_vel.publish(Twist())
# Update the status flag
self.start_test = False
params = {'start_test': False}
dyn_client.update_configuration(params)
rospy.sleep(0.5)
# Stop the robot
self.cmd_vel.publish(Twist())
def __init__(self):
# Creat a node named out_and_back
rospy.init_node('out_and_back', anonymous=False)
# Set rospy to execute a shutdown function when exiting
rospy.on_shutdown(self.shutdown)
# Creat a Publisher, to control the robot's speed topic message_type buffer_size
self.cmd_vel = rospy.Publisher('/cmd_vel', Twist, queue_size=5)
# variable definition
rate = 30 # moving friquency ,How fast will we update the robot's movement?
r = rospy.Rate(
rate) # friquency variable, Set the equivalent ROS rate variable
linear_speed = 0.15 # forward linear speed, Set the forward linear speed to 0.15 m/s
goal_distance = 2.0 # travelled distance, Set the travel distance in meters
angular_speed = 0.5 # rotation speed, Set the rotation speed in radians per second
angular_tolerance = radians(
2.5) # Set the angular tolerance in degrees converted to radians
goal_angle = pi # rotation angle, Set the rotation angle to Pi radians (180 degrees)
# Creat a tf listener
self.tf_listener = tf.TransformListener()
# let ROS rest for 2 seconds,so can give tf some time to fill its buffer
rospy.sleep(2)
self.odom_frame = '/odom' # Set the robot's odometery frame # world-fixed frame
# Find the reference frame
# Find out if the robot uses /base_link or /base_footprint
# /base_footprint frame used by the TurtleBot
# /base_link frame used by Pi Robot and Maxwell
try:
self.tf_listener.waitForTransform(
self.odom_frame, '/base_footprint', rospy.Time(),
rospy.Duration(1.0)) #wait for tramsrform
self.base_frame = '/base_footprint' # the robot uses /base_footprint frame
except (tf.Exception, tf.ConnectivityException,
tf.LookupException): # Exception = error state
try:
self.tf_listener.waitForTransform(self.odom_frame,
'/base_link', rospy.Time(),
rospy.Duration(1.0))
self.base_frame = '/base_link' # the robot uses /base_footprint frame
except (tf.Exception, tf.ConnectivityException,
tf.LookupException):
rospy.loginfo(
"Cannot find transform between /odom and /base_link or /base_footprint"
)
rospy.signal_shutdown("tf Exception")
# Initialize the position variable as a Point type(comes from geometry_msgs.msg which has x,y,z)
position = Point()
# Loop once for each leg of the trip, back and forth
for i in range(2):
# Straight forward
# initialize the movement command with linear velocity
move_cmd = Twist(
) # Initialize the movement command to zero , Twist:linear.x linear.y linear.z Point: x,y,z
move_cmd.linear.x = linear_speed # Set the movement command to forward motion with desired speed
#record the starting position(loop once and loop twice were different)
(position,
rotation) = self.get_odom() # Get the starting position values
x_start = position.x
y_start = position.y
distance = 0 # traveled distance
# travel with a linear speed
while distance < goal_distance and not rospy.is_shutdown(
): # hasno't reached the desired distance
self.cmd_vel.publish(
move_cmd
) # Publish the Twist message velocity comannd until reached the destination
r.sleep()
(position,
rotation) = self.get_odom() # Get the current position
# Compute the Euclidean distance from the start
distance = sqrt(
pow((position.x -
x_start), 2) + # update the traveled distance
pow((position.y - y_start), 2))
# Stop the robot before the rotation
#stop()
move_cmd = Twist() # Initialize the movement command to zero
self.cmd_vel.publish(move_cmd) # publish the stop movement command
rospy.sleep(1) # sleeping
# rotate
move_cmd.angular.z = angular_speed # Set the movement command to a rotation
last_angle = rotation # starting angle, odometry record the last angle measured(starting angle)
turn_angle = 0 # traveled angle
while abs(turn_angle + angular_tolerance) < abs(
goal_angle) and not rospy.is_shutdown(
): # hasno't reached the desired goal_angle
self.cmd_vel.publish(
move_cmd
) # Publish the Twist message velocity comannd angular_speed until reached the desired goal_angle
r.sleep() # sleep with rate
(position,
rotation) = self.get_odom() # Get the current rotation angle
delta_angle = normalize_angle(
rotation - last_angle
) # compute the the delt_angel constraint with in -pi to pi
turn_angle += delta_angle # update the traveled turn_angle
last_angle = rotation # update the last_angle
# Stop the robot before the next leg
#stop()
move_cmd = Twist()
self.cmd_vel.publish(move_cmd)
rospy.sleep(1)
# Stop the robot for good
self.cmd_vel.publish(Twist())